import copy
import math
from datetime import date
import numpy as np
import requests

import logging


GM = 3.986004418e14  # m3/(s2)
Re = 6378137  # m
FLATTENING = 1 / 298.257223563  # Earth flattening, f = (a-b)/a
ECCENTRICITY = 0.0818191908426215  # Earth eccentricy, e2 = 2*f-f^2
E_SQR = ECCENTRICITY ** 2  # squared eccentricity
W_IE = 7292115e-11
VERSION = "1.0"
TWO_PI = 2.0 * math.pi
HALF_PI = 0.5 * math.pi
D2R = math.pi / 180.0
R2D = 180.0 / math.pi
fs = 100.0  # IMU sample frequency
fs_gps = 10.0  # GPS sample frequency
fs_mag = fs  # magnetometer sample frequency, not used for now

static = ["ini lat (deg),ini lon (deg),ini alt (m),ini vx_body (m/s),ini vy_body (m/s),ini vz_body (m/s),"
          "ini yaw (deg),ini pitch (deg),ini roll (deg)", "32,120,0,0,0,0,0,30,0", "command type,yaw (deg),"
                                                                                   "pitch (deg),roll (deg),"
                                                                                   "vx_body (m/s),vy_body (m/s),"
                                                                                   "vz_body (m/s),command duration ("
                                                                                   "s),GPS visibility",
        "1,0,0,0,0,0,0,500,0", ]

modest_mouse = ["ini lat (deg),ini lon (deg),ini alt (m),ini vx_body (m/s),ini vy_body (m/s),ini vz_body (m/s),ini yaw (deg),ini pitch (deg),ini roll (deg)",
"31.508183,120.401989,0,0,0,0,183,0,0",
"command type,yaw (deg),pitch (deg),roll (deg),vx_body (m/s),vy_body (m/s),vz_body (m/s),command duration (s),GPS visibility",
"1,0,0,0,0,0,0,200,1",
"5,0,0,0,9.5,0,0,20,1",
"5,-70,0,0,5,0,0,25,1",
"5,0,0,0,10,0,0,25,1",
"1,0,0,0,0,0,0,16.8,1",
"5,-68,0,0,5,0,0,25,1",
"5,0,0,0,10,0,0,25,1",
"1,0,0,0,0,0,0,17,1",
"5,-90,0,0,5,0,0,25,1",
"5,0,0,0,10,0,0,25,1",
"1,0,0,0,0,0,0,9,1",
"3,12,0,0,0,0,0,5,1",
"1,0,0,0,0,0,0,7,1",
"5,0,0,0,0,0,0,5,1",
"5,-112,0,0,10,0,0,10,1",
"1,0,0,0,0,0,0,8,1",
"1,10,0,0,0,0,0,1,1",
"1,2.8,0,0,0,0,0,10,1",
"1,2,0,0,0,0,0,3,1",
"1,0,0,0,0,0,0,1,1",
"5,80,0,0,5,0,0,10,1",
"5,0,0,0,10,0,0,10,1",
"1,-4,0,0,0,0,0,5,1",
"1,0,0,0,0,0,0,6.5,1",
"5,89,0,0,5,0,0,10,1",
"5,0,0,0,10,0,0,10,1",
"1,0,0,0,0,0,0,9,1",
"5,-94.5,0,0,10,0,0,10,1",
"1,0,0,0,0,0,0,153.5,1",
"5,-88,0,0,10,0,0,10,1",
"1,0,0,0,0,0,0,20,1",
"3,-5,0,0,0,0,0,5,1",
"3,5.6,0,0,0,0,0,5,1",
"5,0,0,0,20,0,0,40,1",
"1,0,0,0,0,0,0,13,1",
"3,3,0,0,0,0,0,5,1",
"5,0,0,0,0,0,0,40,1",
"5,0,0,0,5,0,0,20,1",
"5,-90,0,0,5,0,0,20,1",
"5,0,0,0,8,0,0,20,1",
"5,7,0,0,8,0,0,8,1",
"5,7,0,0,10,0,0,8,1",
"5,-15,0,0,20,0,0,40,1",
"1,0,0,0,0,0,0,6,1",
"5,-0.5,0,0,15,0,0,40,1",
"1,0,0,0,0,0,0,43.5,1",
"5,-90,0,0,10,0,0,40,1",
"5,0,0,0,20,0,0,40,1",
"1,0,0,0,0,0,0,18,1",
"5,0,0,0,5,0,0,40,1",
"5,88,0,0,5,0,0,40,1",
"5,2,0,0,5,0,0,40,1",
"1,0,0,0,0,0,0,9,1",
"5,-16,0,0,5,0,0,40,1",
"1,-1.2,0,0,0,0,0,20,1",
"1,-0.8,0,0,0,0,0,13,1",
"5,96,0,0,5,0,0,40,1",
"5,0,0,0,0,0,0,40,1"]
holland_tunnel = [
        "ini lat (deg),ini lon (deg),ini alt (m),ini vx_body (m/s),ini vy_body (m/s),ini vz_body (m/s),ini yaw (deg),"
        "ini pitch (deg),ini roll (deg)",
        "40.733319,-74.056261,0,0,0,0,-50,0,0",
        "command type,yaw (deg),pitch (deg),roll (deg),vx_body (m/s),vy_body (m/s),vz_body (m/s),command duration ("
        "s),GPS visibility",
        "1,0,0,0,0,0,0,200,1", "5,0,0,0,4,0,0,10,1", "3,90,0,0,0,0,0,15,1", "5,0,0,0,0,0,0,5,1", "5,-15,0,0,7,0,0,4,1",
        "5,0,0,0,7,0,0,3,1", "5,115,0,0,10,0,0,3,1", "2,152,0,0,10,0,0,3,1", "2,138,0,0,20,0,0,10,1",
        "2,144,0,0,20,0,0,5,1", "2,137,0,0,20,0,0,3,1", "1,-6,0,0,0,0,0,1,1", "1,-3,0,0,0,0,0,2,1",
        "1,-5,0,0,0,0,0,5,1", "1,0,0,0,0,0,0,1,1", "1,-7,0,0,0,0,0,2,1", "1,2,0,0,0,0,0,1,1", "1,3,0,0,0,0,0,2,1",
        "1,0,0,0,0,0,0,6,1", "2,97.5,0,0,20,0,0,5,1", "1,0,0,0,0,0,0,7,1", "2,100,0,0,20,0,0,2,1",
        "2,102,0,0,20,0,0,2,1", "2,96,0,0,20,0,0,10,1", "2,98,0,0,20,0,0,10,1", "1,0,0,0,0,0,0,2,1",
        "2,95,0,0,20,0,0,3.5,1", "2,100,0,0,20,0,0,3,1", "2,90,0,0,20,0,0,2,1", "2,98,0,0,20,0,0,5,1",
        "2,107,0,0,20,0,0,2,1", "2,107,0,0,20,0,0,18,0", "1,0,0,0,0,0,0,8,0", "2,95,0,0,20,0,0,4,0",
        "1,0,0,0,0,0,0,15,0", "2,102,0,0,20,0,0,20,0", "1,0,0,0,0,0,0,35,0", "2,97.5,0,0,20,0,0,20,0",
        "1,0,0,0,0,0,0,1.5,0", "2,115,0,0,20,0,0,6,0", "2,135,0,0,20,0,0,20,0", "1,0,0,0,0,0,0,4.4,0",
        "1,0,0,0,-1,0,0,6,1", "1,10,0,0,0,0,0,2,1", "1,20,0,0,0,0,0,2,1", "1,0,0,0,0,0,0,1,1", "1,17,0,0,0,0,0,2,1",
        "1,8,0,0,0,0,0,1.5,1", "1,25,0,0,-1,0,0,1.3,1", "1,0,0,0,-1,0,0,2,1", "2,300,0,0,0,0,0,20,1",
        "1,0,0,0,0,0,0,40,1"]
deg90 = ["ini lat (deg),ini lon (deg),ini alt (m),ini vx_body (m/s),ini vy_body (m/s),ini vz_body (m/s),"
         "ini yaw (deg),ini pitch (deg),ini roll (deg)", "31.9965,120.004,0,10,0,0,315,0,0", "command type,yaw (deg),"
                                                                                             "pitch (deg),roll (deg),"
                                                                                             "vx_body (m/s),"
                                                                                             "vy_body (m/s),"
                                                                                             "vz_body (m/s),"
                                                                                             "command duration (s),"
                                                                                             "GPS visibility",
        "1,0,0,0,0,0,0,1,0", "1,15,0,0,0,0,0,6,0", "1,0,0,0,0,0,0,3,0", ]

motiond = ["ini lat (deg),ini lon (deg),ini alt (m),ini vx_body (m/s),ini vy_body (m/s),ini vz_body (m/s),"
           "ini yaw (deg),ini pitch (deg),ini roll (deg)", "32,120,0,0,0,0,0,0,0", "command type,yaw (deg),"
                                                                                   "pitch (deg),roll (deg),"
                                                                                   "vx_body (m/s),vy_body (m/s),"
                                                                                   "vz_body (m/s),command duration ("
                                                                                   "s),GPS visibility",
        "1,0,0,0,0,0,0,10,0", "5,0,45,0,10,0,0,250,0", "1,0,0,0,0,0,0,10,0", "3,90,-45,0,0,0,0,25,0",
        "1,0,0,0,0,0,0,50,0", "3,180,0,0,0,0,0,25,0", "1,0,0,0,0,0,0,50,0", "3,-180,0,0,0,0,0,25,0",
        "1,0,0,0,0,0,0,50,0", "3,180,0,0,0,0,0,25,0", "1,0,0,0,0,0,0,50,0", "3,-180,0,0,0,0,0,25,0",
        "1,0,0,0,0,0,0,50,0", "3,180,0,0,0,0,0,25,0", "1,0,0,0,0,0,0,50,0", "3,-180,0,0,0,0,0,25,0",
        "1,0,0,0,0,0,0,50,0", "5,0,0,0,0,0,0,10,0", ]

# built-in mobility
high_mobility = np.array([1.0, 0.5, 2.0])  # m/s/s, rad/s/s, rad/s

gyro_low_accuracy = {
        "b": np.array([0.0, 0.0, 0.0]) * D2R, "b_drift": np.array([10.0, 10.0, 10.0]) * D2R / 3600.0,
        "b_corr": np.array([100.0, 100.0, 100.0]), "arw": np.array([0.75, 0.75, 0.75]) * D2R / 60.0,
}
accel_low_accuracy = {
        "b": np.array([0.0e-3, 0.0e-3, 0.0e-3]), "b_drift": np.array([2.0e-4, 2.0e-4, 2.0e-4]),
        "b_corr": np.array([100.0, 100.0, 100.0]), "vrw": np.array([0.05, 0.05, 0.05]) / 60.0,
}
mag_low_accuracy = {
        "si": np.eye(3) + np.random.randn(3, 3) * 0.0, "hi": np.array([10.0, 10.0, 10.0]) * 0.0,
        "std": np.array([0.1, 0.1, 0.1]),
}
# mid accuracy, partly from IMU381
gyro_mid_accuracy = {
        "b": np.array([0.0, 0.0, 0.0]) * D2R, "b_drift": np.array([3.5, 3.5, 3.5]) * D2R / 3600.0,
        "b_corr": np.array([100.0, 100.0, 100.0]), "arw": np.array([0.25, 0.25, 0.25]) * D2R / 60,
}
accel_mid_accuracy = {
        "b": np.array([0.0e-3, 0.0e-3, 0.0e-3]), "b_drift": np.array([5.0e-5, 5.0e-5, 5.0e-5]),
        "b_corr": np.array([100.0, 100.0, 100.0]), "vrw": np.array([0.03, 0.03, 0.03]) / 60,
}
mag_mid_accuracy = {
        "si": np.eye(3) + np.random.randn(3, 3) * 0.0, "hi": np.array([10.0, 10.0, 10.0]) * 0.0,
        "std": np.array([0.01, 0.01, 0.01]),
}
# high accuracy, partly from HG9900, partly from
# http://www.dtic.mil/get-tr-doc/pdf?AD=ADA581016
gyro_high_accuracy = {
        "b": np.array([0.0, 0.0, 0.0]) * D2R, "b_drift": np.array([0.1, 0.1, 0.1]) * D2R / 3600.0,
        "b_corr": np.array([100.0, 100.0, 100.0]), "arw": np.array([2.0e-3, 2.0e-3, 2.0e-3]) * D2R / 60,
}
accel_high_accuracy = {
        "b": np.array([0.0e-3, 0.0e-3, 0.0e-3]), "b_drift": np.array([3.6e-6, 3.6e-6, 3.6e-6]),
        "b_corr": np.array([100.0, 100.0, 100.0]), "vrw": np.array([2.5e-5, 2.5e-5, 2.5e-5]) / 60,
}
mag_high_accuracy = {
        "si": np.eye(3) + np.random.randn(3, 3) * 0.0, "hi": np.array([10.0, 10.0, 10.0]) * 0.0,
        "std": np.array([0.001, 0.001, 0.001]),
}

## built-in GPS error profiles
gps_low_accuracy = {
        "stdp": np.array([5.0, 5.0, 7.0]), "stdv": np.array([0.05, 0.05, 0.05]),
}

## built-in odometer error profiles
odo_low_accuracy = {"scale": 0.99, "stdv": 0.1}


class IMU(object):
    """
    IMU class
    """

    def __init__(self, accuracy="low-accuracy", axis=6, gps=True, gps_opt=None, odo=False, odo_opt=None, ):
        """
        Args:
            accuracy: IMU grade.
                This can be a string to use the built-in IMU models:
                    'low-accuracy':
                    'mid-accuracy':
                    'high-accuracy':
                or a dictionary to custom the IMU model:
                    'gyro_b': gyro bias, deg/hr
                    'gyro_arw': gyro angle random walk, deg/rt-hr
                    'gyro_b_stability': gyro bias instability, deg/hr
                    'gyro_b_corr': gyro bias isntability correlation time, sec
                    'accel_b': accel bias, m/s2
                    'accel_vrw' : accel velocity random walk, m/s/rt-hr
                    'accel_b_stability': accel bias instability, m/s2
                    'accel_b_corr': accel bias isntability correlation time, sec
                    'mag_si': soft iron, default is a 3x3 identity matrix.
                    'mag_hi': hard iron, default is 3x1 zero vector.
                    'mag_std': mag noise std.
                If magnetometer is enabled and you want to specify the magnetic params,
                soft iron and hard iron are optional and noise std is needed. If soft
                iron or hard iron is not provided, defautls will be used.
            axis: 6 for IMU, 9 for IMU+magnetometer
            gps: True if GPS exists, False if not.
            gps_opt: a dictionary to specify the GPS error model.
                'stdp': position RMS error, meters
                'stdv': vertical RMS error, meters/second
            odo: True if odometer exists, False if not.
            odo_opt: a dictionary to specify the odometer error model.
                'scale': scale factor
                'stdv': velocity measurement noise, meters/second.
        """
        # check axis
        self.magnetometer = False
        if axis == 9:
            self.magnetometer = True
        elif axis != 6:
            raise ValueError("axis should be either 6 or 9.")

        # built-in imu error model
        self.gyro_err = gyro_low_accuracy  # default is low accuracy
        self.accel_err = accel_low_accuracy
        self.mag_err = mag_low_accuracy

        # accuracy is a string, use built-in models
        if isinstance(accuracy, str):
            if accuracy == "low-accuracy":  # 'low-accuracy'
                pass
            elif accuracy == "mid-accuracy":  # 'mid-accuracy'
                self.gyro_err = gyro_mid_accuracy
                self.accel_err = accel_mid_accuracy
                self.mag_err = mag_mid_accuracy
            elif accuracy == "high-accuracy":  # 'high-accuracy'
                self.gyro_err = gyro_high_accuracy
                self.accel_err = accel_high_accuracy
                self.mag_err = mag_high_accuracy
            else:  # not a valid string
                raise ValueError("accuracy is not a valid string.")
        # accuracy is a dict, user defined models
        elif isinstance(accuracy, dict):
            # set IMU and mag error params
            if (
                    "gyro_b" in accuracy and "gyro_b_stability" in accuracy and "gyro_arw" in accuracy and "accel_b"
                    in accuracy and "accel_b_stability" in accuracy and "accel_vrw" in accuracy):  # check keys in
                # accuracy
                # required parameters
                self.gyro_err["b"] = accuracy["gyro_b"] * D2R / 3600.0
                self.gyro_err["b_drift"] = accuracy["gyro_b_stability"] * D2R / 3600.0
                self.gyro_err["arw"] = accuracy["gyro_arw"] * D2R / 60.0
                self.accel_err["b"] = accuracy["accel_b"]
                self.accel_err["b_drift"] = accuracy["accel_b_stability"]
                self.accel_err["vrw"] = accuracy["accel_vrw"] / 60.0
                if self.magnetometer:  # at least noise std should be specified
                    if "mag_std" in accuracy:
                        self.mag_err["std"] = accuracy["mag_std"]
                    else:
                        raise ValueError("Magnetometer is enabled, " + "but its noise std is not specified.")
                # optional parameters
                if "gyro_b_corr" in accuracy:
                    self.gyro_err["b_corr"] = accuracy["gyro_b_corr"]
                else:
                    self.gyro_err["b_corr"] = np.array([float("inf"), float("inf"), float("inf")])
                if "accel_b_corr" in accuracy:
                    self.accel_err["b_corr"] = accuracy["accel_b_corr"]
                else:
                    self.accel_err["b_corr"] = np.array([float("inf"), float("inf"), float("inf")])
                if "mag_si" in accuracy:
                    self.mag_err["si"] = accuracy["mag_si"]
                else:
                    self.mag_err["si"] = np.eye(3)
                if "mag_hi" in accuracy:
                    self.mag_err["hi"] = accuracy["mag_hi"]
                else:
                    self.mag_err["hi"] = np.array([0.0, 0.0, 0.0])
            # raise error when required keys are missing
            else:
                raise ValueError(
                        "accuracy should at least have keys: \n" + "gyro_b, gyro_b_stability, gyro_arw, " + "accel_b, "
                                                                                                            "accel_b_stability and accel_vrw")
        else:
            raise TypeError("accuracy is not valid.")

        # build GPS model
        if gps:
            self.gps = True
            if gps_opt is None:
                self.gps_err = gps_low_accuracy
            elif isinstance(gps_opt, dict):
                if "stdp" in gps_opt and "stdv" in gps_opt:
                    self.gps_err = gps_opt
                else:
                    raise ValueError("gps_opt should have key: stdp and stdv")
            else:
                raise TypeError("gps_opt should be None or a dict")
        else:
            self.gps = False
            self.gps_err = None

        # build odometer model
        if odo:
            self.odo = True
            if odo_opt is None:
                self.odo_err = odo_low_accuracy
            elif isinstance(odo_opt, dict):
                if "scale" in odo_opt and "stdv" in odo_opt:
                    self.odo_err = odo_opt
                else:
                    raise ValueError("odo_opt should have key: scale and stdv")
            else:
                raise TypeError("odo_opt should be None or a dict")
        else:
            self.odo = False
            self.odo_err = None

    def set_gyro_error(self, gyro_error="low-accuracy"):
        """
        set gyro error model
        Args:
            gyro_error: gyro error model.
                This can be a string to use the built-in gyro models:
                    'low-accuracy':
                    'mid-accuracy':
                    'high-accuracy':
                or a dictionary to custom the IMU model:
                    'b': gyro bias, deg/hr
                    'arw': gyro angle random walk, deg/rt-hr
                    'b_drift': gyro bias instability, deg/hr
                    'b_corr': gyro bias isntability correlation time, sec
        """
        if isinstance(gyro_error, str):
            if gyro_error == "low-accuracy":  # 'low-accuracy'
                self.gyro_err = gyro_low_accuracy
            elif gyro_error == "mid-accuracy":  # 'mid-accuracy'
                self.gyro_err = gyro_mid_accuracy
            elif gyro_error == "high-accuracy":  # 'high-accuracy'
                self.gyro_err = gyro_high_accuracy
            else:  # not a valid string
                raise ValueError("gyro_error is not a valid string.")
        # accuracy is a dict, user defined models
        elif isinstance(gyro_error, dict):
            for i in gyro_error:
                if i in self.gyro_err:
                    self.gyro_err[i] = gyro_error[i]
                else:
                    raise ValueError("unsupported key: %s in gyro_error" % i)
        else:
            raise TypeError("gyro_error is not valid.")

    def set_accel_error(self, accel_error="low-accuracy"):
        """
        set accel error model
        Args:
            accel_error: accel error model.
                This can be a string to use the built-in accel models:
                    'low-accuracy':
                    'mid-accuracy':
                    'high-accuracy':
                or a dictionary to custom the IMU model:
                    'b': accel bias, m/s2
                    'vrw' : accel velocity random walk, m/s/rt-hr
                    'b_drift': accel bias instability, m/s2
                    'b_corr': accel bias isntability correlation time, sec
        """
        if isinstance(accel_error, str):
            if accel_error == "low-accuracy":  # 'low-accuracy'
                self.accel_err = accel_low_accuracy
            elif accel_error == "mid-accuracy":  # 'mid-accuracy'
                self.accel_err = accel_mid_accuracy
            elif accel_error == "high-accuracy":  # 'high-accuracy'
                self.accel_err = accel_high_accuracy
            else:  # not a valid string
                raise ValueError("accel_error is not a valid string.")
        # accuracy is a dict, user defined models
        elif isinstance(accel_error, dict):
            for i in accel_error:
                if i in self.accel_err:
                    self.accel_err[i] = accel_error[i]
                else:
                    raise ValueError("unsupported key: %s in accel_error" % i)
        else:
            raise TypeError("accel_error is not valid.")

    def set_gps(self, gps_error=None):
        """
        set GPS error model
        Args:
            gps_error: GPS error model
                This can be either none or a dictionary to specify the GPS error model:
                    'stdp': position RMS error, meters
                    'stdv': velocity RMS error, meters/second
        """
        if not self.gps:
            return
        if gps_error is None:
            self.gps_err = gps_low_accuracy
        elif isinstance(gps_error, dict):
            if "stdp" in gps_error and "stdv" in gps_error:
                self.gps_err = gps_error
            else:
                raise ValueError("gps_error should have key: stdp and stdv")
        else:
            raise TypeError("gps_error should be None or a dict")

    def set_odo(self, odo_error=None):
        """
        set odometer error model
        Args:
            odo_error: odometer error model
                This can be either none or a dictionary to specify the odometer error model:
                    'scale': scale factor
                    'stdv': velocity RMS error, meters/second
        """
        if not self.gps:
            return
        if odo_error is None:
            self.odo_err = odo_low_accuracy
        elif isinstance(odo_error, dict):
            if "stdp" in odo_error and "stdv" in odo_error:
                self.gps_err = odo_error
            else:
                raise ValueError("odo_error should have key: stdp and stdv")
        else:
            raise TypeError("odo_error should be None or a dict")

    def set_mag_error(self, mag_error="low-accuracy"):
        """
        set magnetometer error model
        Args:
            mag_error: magnetometer error model.
                This can be a string to use the built-in mag models:
                    'low-accuracy':
                    'mid-accuracy':
                    'high-accuracy':
                or a dictionary to custom the IMU model:
                    'si': soft iron, default is a 3x3 identity matrix.
                    'hi': hard iron, default is 3x1 zero vector.
                    'std': mag noise std.
        """
        if not self.magnetometer:
            return
        if isinstance(mag_error, str):
            if mag_error == "low-accuracy":  # 'low-accuracy'
                self.mag_err = mag_low_accuracy
            elif mag_error == "mid-accuracy":  # 'mid-accuracy'
                self.mag_err = mag_mid_accuracy
            elif mag_error == "high-accuracy":  # 'high-accuracy'
                self.mag_err = mag_high_accuracy
            else:  # not a valid string
                raise ValueError("mag_error is not a valid string.")
        # accuracy is a dict, user defined models
        elif isinstance(mag_error, dict):
            for i in mag_error:
                if i in self.mag_err:
                    self.mag_err[i] = mag_error[i]
                else:
                    raise ValueError("unsupported key: %s in mag_error" % i)
        else:
            raise TypeError("mag_error is not valid.")


def __convert_pos(data, units, ref_frame):
    """
    Convert position data into a proper form.
    For example, if units are [deg deg m] or [rad rad m] and ref_frame is 1, convertion
    is needed. LLA form position will be converted to [x y z] form. Vice Versa.
    Args:
        data: nx3 numpy array, can be in [Lat Lon Alt] or [x y z] form.
        units: units of the data.
        ref_frame: reference frame of the simulation. 0:NED, 1:virtual inertial
    Returns:
        data: nx3 numpy array after convertion.
        units: units of converted dta
    """
    if ref_frame == 1:
        # deg to rad
        if units == ["deg", "deg", "m"]:
            units = ["rad", "rad", "m"]
            data[:, 0] = data[:, 0] * D2R
            data[:, 1] = data[:, 1] * D2R
        # lla2ned
        if units == ["rad", "rad", "m"]:
            units = ["m", "m", "m"]
            # relative motion in ECEF
            data = lla2ecef_batch(data)
            ini_pos_ecef = data[0, :]  # initial ECEF position
            data = data - ini_pos_ecef
            # relative motion in ECEF to NED, NED defined by first LLA
            c_ne = ecef_to_ned(data[0, 0], data[0, 1])
            data = data.dot(c_ne.T)
            data = data + ini_pos_ecef
    elif ref_frame == 0:
        # ned2lla or ecef2lla
        #  Because if the data are in NED or ECEF is unknown, this is not supported.
        if units == ["m", "m", "m"]:
            units = ["rad", "rad", "m"]
            logging.debug("Unsupported position conversion from xyz to LLA.")
    return data, units


def parse_mode(mode):
    """
    Parse mode. Not completely implemented yet.
    Args:
        mode: simualtion mode
    """
    if mode is not None:
        if isinstance(mode, str):  # choose built-in mode
            mode = mode.lower()
            if "flight" in mode:
                mobility = high_mobility
            elif "land" in mode:
                mobility = high_mobility
            elif "ship" in mode:
                mobility = high_mobility
            else:
                mobility = high_mobility
        elif isinstance(mode, np.ndarray):  # customize the sim mode
            if mode.shape == (3,):
                mobility = mode
                mobility[1] *= D2R
                mobility[2] *= D2R
            else:
                raise TypeError("mode should be of size (3,)")
        else:
            raise TypeError("mode should be a string or a numpy array of size (3,)")
    else:
        mobility = high_mobility
    return mobility


def __get_data_units(line):
    """
    Get data units in file. Units information should be provided in the first row.
    Args:
        line
    Returns:
        units: a list of units corresponding to each column in the file. If not untis
            found, units is None.
    """
    units = None

    tmp_units = []
    for i in line:
        left_bracket = i.find("(")
        right_bracket = i.rfind(")")
        if left_bracket != -1 and right_bracket != -1 and right_bracket > left_bracket:
            tmp_units.append(i[left_bracket + 1: right_bracket])
    if len(tmp_units) == len(line):
        units = tmp_units
    return units


class Sim(object):
    """
    INS simulation engine.
    """

    def __init__(self, fs, mdef, ref_frame=0, imu=None, mode=None, env=None, algorithm=None):
        """
        Args:
            fs: [fs_imu, fs_gps, fs_mag], Hz.
                fs_imu: The sample rate of IMU. This is also the sampel rate of the simulatino.
                fs_gps: The sample rate of GPS.
                fs_mag: not used now. The sample rate of the magnetometer is
                    the same as that of the imu.

            mdef: If you want to do simulation with logged data files, motion_def should be
                a directory contains the data files. Data files should be named as data_name.csv.
                Supported data names are algorithm input. (Refer to readme.md)
                If you do not have logged data files and want to generate sensor data from a motion
                definition file,  motion_def should be a .csv file to define the waypoints.
                The .csv file should be organized as follows:
                row 1: header line for initial states
                row 2: initial states, which include:
                    col 1-3: initial position (LLA, deg, meter),
                    col 4-6: initial velocity in body frame(m/s),
                    col 7-9: initial attitude (Euler angles that rotate the reference frame to the
                        body frame according to the ZYX rotation sequence, deg).
                row 3: header line for motion command
                row >=4: motion commands, which include
                    col 1: motion type. The following types are supported:
                        1: Euler angles change rate and body frame velocity change rate.
                        2: absolute att and absolute vel to reach.
                        3: relative att and vel change.
                        4: absolute att, relative vel.
                        5: relative att, absolute vel.
                    col 2-7: motion command (deg, m/s).
                        [yaw, pitch, roll, vx (velocity along body x axis), vy, vz].
                        For motion type 1, the above Euler angles and velocity should be
                        change rate, corresponding units are (deg/s, m/s/s).
                    col 8: maximum time for the given segment, sec. Max time together with the
                        param "mode" determines if this command can be executed successfully.
                        If actual executing time is less than max time, the remaining time will
                        not be used and the next command will be executed immediately. If the
                        command cannot be finished within max time, the next command will be
                        executed after max time. If you want to fully control execution time by
                        your own, you should always choose motion type to be 1.
                    col 9: gps visibility, should be 1 or 0.

            ref_frame: reference frame used as the navigation frame and the attitude reference.
                        0: NED (default), with x axis pointing along geographic north,
                           y axis pointing eastward,
                           z axis pointing downward.
                           Position will be expressed in LLA form, and the reference velocity of
                           the vehicle relative to the ECEF frame will be expressed in the NED
                           frame, and GPS velocity will be expressed in the NED frame.
                        1: a virtual inertial frame with constant g,
                           x axis pointing along geographic/magnetic north,
                           z axis pointing along g,
                           y axis completing a right-handed coordinate system.
                           Position and velocity will both be in the [x y z] form in this frame.
                           **Notice: For this virtual inertial frame, position is indeed the sum of
                           the initial position in ecef and the relative position in the virutal
                           inertial frame. Indeed, two vectors expressed in different frames should
                           not be added. This is done in this way here just to preserve all useful
                           information to generate .kml files.
                           Keep this in mind if you use this result.

            imu: Define the IMU error model. See IMU in imu_model.py.
                If you want to do simulation with logged data files, set imu=None.
                If you do not have logged data files and want to generate sensor data from a motion
                definition file, you should specify the IMU model.

            mode: simulation mode could be a string to specify a built-in mode:
                'flight':
                ...
                This is not implemented yet. A built-in 'high_mobility' mode is used.
                or a numpy array of size (3,) to customize the sim mode.
                    [max_acceleration, max_angular_acceleration, max_angular_velocity],
                    in units of [m/s/s, deg/s/s, deg/s]

            env: vibration model. There are three kinds of vibration models:
                '[nx ny nz]g-random': normal-distribution random vibration, rms is n*9.8 m/s^2
                '[nx ny nz]-random': normal-distribution random vibration, rms is n m/s^2
                '[nx ny nz]g-mHz-sinusoidal': sinusoidal vibration of m Hz, amplitude is n*9.8 m/s^2
                '[nx ny nz]-mHz-sinusoidal': sinusoidal vibration of m Hz, amplitude is n m/s^2
                numpy array of (n,4): single-sided PSD. Each row is [freq, x, y, z], m^2/s^4/Hz

            algorithm: a user defined algorithm or list of algorithms. If there are multiple
                algorithms, all algorithms should have the same input and output.
        """
        self.fs = fs
        self.imu = imu
        self.mode = mode
        self.env = env
        if ref_frame == 0 or ref_frame == 1:
            self.ref_frame = ref_frame
        else:
            self.ref_frame = 0  # default frame is NED
        # simulation status
        self.sim_count = 1  # simulation count
        self.sim_complete = False  # simulation complete successfully
        self.sim_results = False  # simulation results is generated
        # simulation data manager
        self.dmgr = InsDataMgr(fs, self.ref_frame)
        if mdef == "90degrees":
            self.data_src = deg90
        elif mdef == "motion_def":
            self.data_src = motiond
        else:

            self.data_src = static
        self.data_from_files = False
        # algorithm manager
        self.amgr = InsAlgoMgr(algorithm)

        # associated data mapping. this is a dict in the following form:
        #   {'dst_name': ['src_name', routine_convert_src_to_dst]}
        self.data_map = {
                self.dmgr.ref_att_euler.name: [self.dmgr.ref_att_quat.name, self.__quat2euler_zyx, ],
                self.dmgr.ref_att_quat.name: [self.dmgr.ref_att_euler.name, self.__euler2quat_zyx, ],
                self.dmgr.att_euler.name: [self.dmgr.att_quat.name, self.__quat2euler_zyx],
                self.dmgr.att_quat.name: [self.dmgr.att_euler.name, self.__euler2quat_zyx],
        }

        # error terms we are interested in
        self.interested_error = {
                self.dmgr.att_euler.name: "angle", self.dmgr.pos.name: None, self.dmgr.vel.name: None,
        }

        # summary
        self.sum = ""

    def run(self, num_times=1):
        """
        run simulation.
        Args:
            num_times: run the simulation for num_times times with given IMU error model.
        """
        self.sim_count = int(num_times)
        if self.sim_count < 1:
            self.sim_count = 1

        self.__gen_data_from_pathgen()

        if self.amgr.algo is not None:
            # tell data manager the output of the algorithm
            self.dmgr.set_algo_output(self.amgr.output)
            # get algo input data
            algo_input = self.dmgr.get_data(self.amgr.input)
            # run the algo and get algo output
            algo_output = self.amgr.run_algo(algo_input, range(self.sim_count))
            # add algo output to ins_data_manager
            for i in range(len(self.amgr.output)):
                self.dmgr.add_data(self.amgr.output[i], algo_output[i])
        # simulation complete successfully
        self.sim_complete = True

    def results(self):
        """
        Simulation results.
        Save results to .csv files containing all data generated.
        Simulation results also include a summary file containing statistics of the simulation.

        Returns: a dict contains all simulation results.
        """
        if self.sim_complete:

            self.__add_associated_data_to_results()

            self.sim_results = True

            return self.dmgr.available
        else:
            logging.debug("Call Sim.run() to run the simulaltion first.")
            return None

    def __quat2euler_zyx(self, src):
        """
        quaternion to Euler angles (zyx)
        """
        if isinstance(src, np.ndarray):
            n = src.shape[0]
            dst = np.zeros((n, 3))
            for j in range(n):
                dst[j, :] = quat2euler(src[j, :])
            return dst
        elif isinstance(src, dict):
            dst = {}
            for i in src:
                n = src[i].shape[0]
                euler = np.zeros((n, 3))
                for j in range(n):
                    euler[j, :] = quat2euler(src[i][j, :])
                dst[i] = euler
            return dst
        else:
            raise ValueError("%s is not a dict or numpy array." % src.name)

    def __gen_data_from_pathgen(self):
        """
        Generate data from pathgen.
        """
        # read motion definition
        [ini_pva, motion_def] = self.__parse_motion()
        # output definitions
        output_def = np.array([[1.0, self.fs[0]], [1.0, self.fs[0]], [1.0, self.fs[0]]])
        if self.imu.gps:
            output_def[1, 0] = 1.0
            output_def[1, 1] = self.fs[1]
        else:
            output_def[1, 0] = -1.0
        if self.imu.odo:
            output_def[2, 0] = 1.0
        else:
            output_def[2, 0] = -1.0
        # sim mode-->vehicle maneuver capability
        mobility = parse_mode(self.mode)

        # generate reference data and add data to ins_data_manager
        rtn = path_gen(ini_pva, motion_def, output_def, mobility, self.ref_frame, self.imu.magnetometer, )
        self.dmgr.add_data(self.dmgr.time.name, rtn["nav"][:, 0] / self.fs[0])
        self.dmgr.add_data(self.dmgr.ref_pos.name, rtn["nav"][:, 1:4])
        self.dmgr.add_data(self.dmgr.ref_vel.name, rtn["nav"][:, 4:7])
        self.dmgr.add_data(self.dmgr.ref_att_euler.name, rtn["nav"][:, 7:10])
        self.dmgr.add_data(self.dmgr.ref_accel.name, rtn["imu"][:, 1:4])
        self.dmgr.add_data(self.dmgr.ref_gyro.name, rtn["imu"][:, 4:7])
        if self.imu.gps:
            self.dmgr.add_data(self.dmgr.gps_time.name, rtn["gps"][:, 0] / self.fs[0])
            self.dmgr.add_data(self.dmgr.ref_gps.name, rtn["gps"][:, 1:7])
            self.dmgr.add_data(self.dmgr.gps_visibility.name, rtn["gps"][:, 7])
        if self.imu.magnetometer:
            self.dmgr.add_data(self.dmgr.ref_mag.name, rtn["mag"][:, 1:4])
        if self.imu.odo:
            self.dmgr.add_data(self.dmgr.ref_odo.name, rtn["odo"][:, 2])
        # generate sensor data
        # environment-->vibraition params
        vib_def = self.__parse_env(self.env)
        for i in range(self.sim_count):
            accel = acc_gen(self.fs[0], self.dmgr.ref_accel.data, self.imu.accel_err, vib_def)
            self.dmgr.add_data(self.dmgr.accel.name, accel, key=i)
            gyro = gyro_gen(self.fs[0], self.dmgr.ref_gyro.data, self.imu.gyro_err)
            self.dmgr.add_data(self.dmgr.gyro.name, gyro, key=i)
            if self.imu.gps:
                gps = gps_gen(self.dmgr.ref_gps.data, self.imu.gps_err, self.ref_frame)
                self.dmgr.add_data(self.dmgr.gps.name, gps, key=i)
            if self.imu.magnetometer:
                mag = mag_gen(self.dmgr.ref_mag.data, self.imu.mag_err)
                self.dmgr.add_data(self.dmgr.mag.name, mag, key=i)
            if self.imu.odo:
                odo = odo_gen(self.dmgr.ref_odo.data, self.imu.odo_err)
                self.dmgr.add_data(self.dmgr.odo.name, odo, key=i)

    def __data_from_algo_output(self, data_name):
        """
        Check if data corresponding to data_name are from algo output or associated
        data of algo output. For example, if an algo outputs quaternions, euler angles
        can be calculated from quaternions accordig to self.data_map.
        Args:
            data_name: a string data name.
        Returns: A list of True or False. Its length is equal to the number of algos.
            True of false representing if this data of data_name are or can be
            calculated from the output of this algo.
        """
        rtn = []
        for i in range(self.amgr.nalgo):
            algo_output = self.amgr.algo[i].output
            rtn.append(data_name in algo_output or data_name in self.data_map and self.data_map[data_name][
                0] in algo_output)
        return rtn

    def __parse_motion(self):
        """
        Get initial pos/vel/att and motion command from a .csv file specified by self.data_src.
        Returns: a list containing:
            inis_pos_n: initial position, [Lat Lon Alt], units: rad, rad, m.
            ini_vel_b: initial velocity in the body frame, units: m/s.
            ini_att: initial Euler angles, ZYX rotation sequence, units: rad
            motion_def: motion commands, units: rad, rad/s, m, m/s.
        """
        try:
            ini_state = np.genfromtxt(self.data_src, delimiter=",", skip_header=1, max_rows=1)
            waypoints = np.genfromtxt(self.data_src, delimiter=",", skip_header=3)
        except:
            raise ValueError("motion definition file must have nine columns \
                              and at least four rows (two header rows + at least two data rows).")
        ini_pos_n = ini_state[0:3]
        ini_pos_n[0] *= D2R
        ini_pos_n[1] *= D2R
        ini_vel_b = ini_state[3:6]
        ini_att = ini_state[6:9] * D2R
        if waypoints.ndim == 1:  # if waypoints is of size (n,), change it to (1,n)
            waypoints = waypoints.reshape((1, len(waypoints)))
        motion_def = waypoints[:, [0, 1, 2, 3, 4, 5, 6, 7, 8]]
        # convert deg to rad
        motion_def[:, 1:4] = motion_def[:, 1:4] * D2R
        # replace nan with 0.0, doing this to be compatible with older version motion def files.
        motion_def[np.isnan(motion_def)] = 0.0

        return [np.hstack((ini_pos_n, ini_vel_b, ini_att)), motion_def]

    def __parse_env(self, env):
        """
        Parse env.
        Args:
            env: vibration model
        """
        vib_def = {}
        if env is None:
            vib_def = None
        elif isinstance(env, str):  # specify simple vib model
            env = env.lower()
            if "random" in env:  # normal distribution
                vib_def["type"] = "random"
                env = env.replace("-random", "")
            elif "sinusoidal" in env:  # sinusoidal vibration
                vib_def["type"] = "sinusoidal"
                env = env.replace("-sinusoidal", "")
                if env[-2:] == "hz":
                    try:
                        idx_first_mark = env.find("-")
                        vib_def["freq"] = math.fabs(float(env[idx_first_mark + 1: -2]))
                        env = env[:idx_first_mark]
                    except:
                        raise ValueError("env = '%s' is not valid (invalid vib freq)." % env)
                else:
                    raise ValueError("env = '%s' is not valid (No vib freq)." % env)
            else:
                raise ValueError("env = '%s' is not valid." % env)
            vib_amp = (1.0)  # vibration amplitude, 1sigma for random, peak value for sinusoidal
            if env[-1] == "g" or env[-1] == "G":
                vib_amp = 9.8
                env = env[:-1]  # remove 'g' or 'G'
            try:
                env = env[1:-1]  # remove '[]' or '()'
                env = env.split(" ")
                vib_amp *= np.array(env, dtype="float64")
                vib_def["x"] = vib_amp[0]
                vib_def["y"] = vib_amp[1]
                vib_def["z"] = vib_amp[2]
            except:
                raise ValueError("Cannot convert '%s' to float" % env)
        elif isinstance(env, np.ndarray):  # customize the vib model with PSD
            if env.ndim == 2 and env.shape[1] == 4:  # env is a np.array of size (n,4)
                vib_def["type"] = "psd"
                n = env.shape[0]
                half_fs = 0.5 * self.fs.data
                if env[-1, 0] > half_fs:
                    n = np.where(env[:, 0] > half_fs)[0][0]
                vib_def["freq"] = env[:n, 0]
                vib_def["x"] = env[:n, 1]
                vib_def["y"] = env[:n, 2]
                vib_def["z"] = env[:n, 3]
            else:
                raise TypeError("env should be of size (n,2)")
        else:
            raise TypeError("env should be a string or a numpy array of size (n,2)")
        return vib_def

    def __euler2quat_zyx(self, src):
        """
        Euler angles (zyx) to quaternion
        """
        # array
        if isinstance(src, np.ndarray):
            n = src.shape[0]
            dst = np.zeros((n, 4))
            for j in range(n):
                dst[j, :] = euler2quat(src[j, :])
            return dst
        # dict557
        elif isinstance(src, dict):
            dst = {}
            for i in src:
                n = src[i].shape[0]
                quat = np.zeros((n, 4))
                for j in range(n):
                    quat[j, :] = euler2quat(src[i][j, :])
                dst[i] = quat
            return dst
        else:
            raise ValueError("%s is not a dict or numpy array." % src.name)

    def __add_associated_data_to_results(self):
        """
        Check if some data in self.res have associated data. If so, calculate the associated data
        and add the data in self.res.
        For example, pathgen generates Euler angles, this procedure will calculate the
        coresponding quaternions and add those in self.res.
        """
        for i in self.data_map:
            # data available and its associated data are supported
            src_name = self.data_map[i][0]
            if src_name in self.dmgr.available and self.dmgr.is_supported(i):
                src_data = self.dmgr.get_data([src_name])[0]
                # src_data is a dict, add associated data of all keys
                if isinstance(src_data, dict):
                    for key in src_data:
                        if not self.dmgr.is_available(i, key):
                            self.dmgr.add_data(i, self.data_map[i][1](src_data[key]), key)
                else:
                    if not self.dmgr.is_available(i):
                        self.dmgr.add_data(i, self.data_map[i][1](src_data))


class InsDataMgr(object):
    """
    A class that manage all data generated in an INS solution. For example, reference data,
    sensor data, algorithm results. These data can be saved to files or plot in figures.
    """

    def __init__(self, fs, ref_frame=0):
        """
        Args:
            fs: [fs_imu, fs_gps, fs_mag], Hz.
                fs_imu: The sample rate of IMU.
                fs_gps: The sample rate of GPS.
                fs_mag: not used now. The sample rate of the magnetometer is
                    the same as that of the imu.

            ref_frame: reference frame used as the navigation frame,
                        0: NED (default), with x axis pointing along geographic north,
                            y axis pointing eastward,
                            z axis pointing downward.
                        1: a virtual inertial frame with constant g,
                            x axis pointing along magnetic north,
                            z axis pointing along g,
                            y axis completing a right-handed coordinate system.
                            Notice: For this virtual inertial frame, position is indeed the sum of
                            the initial position in ecef and the relative position in the virutal
                            inertial frame.
        """
        # sample rate
        self.fs = Sim_data(name="fs", description="Sample frequency of IMU", units=["Hz"], plottable=False, )
        self.fs_gps = Sim_data(name="fs_gps", description="Sample frequency of GPS", units=["Hz"], plottable=False, )
        self.fs_mag = Sim_data(name="fs_mag", description="Sample frequency of Magnetometer", units=["Hz"],
                plottable=False, )
        # reference frame
        self.ref_frame = Sim_data(name="ref_frame", description="Reference frame", plottable=False)
        if ref_frame == 0 or ref_frame == 1:
            self.ref_frame.data = ref_frame
        else:
            self.ref_frame.data = 0  # default frame is NED

        ########## possible data generated by simulation ##########
        # reference data
        self.time = Sim_data(name="time", description="sample time", units=["sec"])
        self.gps_time = Sim_data(name="gps_time", description="GPS sample time", units=["sec"])
        self.gps_visibility = Sim_data(name="gps_visibility", description="GPS visibility")
        self.ref_pos = Sim_data(name="ref_pos", description="true LLA pos in the navigation frame",
                units=["rad", "rad", "m"], output_units=["deg", "deg", "m"],
                legend=["ref_pos_lat", "ref_pos_lon", "ref_pos_alt"], )
        self.ref_vel = Sim_data(name="ref_vel", description="true vel in the NED frame", units=["m/s", "m/s", "m/s"],
                legend=["ref_vel_x", "ref_vel_y", "ref_vel_z"], )
        self.ref_att_euler = Sim_data(name="ref_att_euler", description="true attitude (Euler angles, ZYX)",
                units=["rad", "rad", "rad"], output_units=["deg", "deg", "deg"],
                legend=["ref_Yaw", "ref_Pitch", "ref_Roll"], )
        self.ref_att_quat = Sim_data(name="ref_att_quat", description="true attitude (quaternion)",
                legend=["q0", "q1", "q2", "q3"], )
        self.ref_gyro = Sim_data(name="ref_gyro", description="true angular velocity in the body frame",
                units=["rad/s", "rad/s", "rad/s"], output_units=["deg/s", "deg/s", "deg/s"],
                legend=["ref_gyro_x", "ref_gyro_y", "ref_gyro_z"], )
        self.ref_accel = Sim_data(name="ref_accel", description="true accel in the body frame",
                units=["m/s^2", "m/s^2", "m/s^2"], legend=["ref_accel_x", "ref_accel_y", "ref_accel_z"], )
        self.ref_gps = Sim_data(name="ref_gps", description="true GPS LLA position and NED velocity",
                units=["rad", "rad", "m", "m/s", "m/s", "m/s"], output_units=["deg", "deg", "m", "m/s", "m/s", "m/s"],
                legend=["ref_gps_lat", "ref_gps_lon", "ref_gps_alt", "ref_gps_vN", "ref_gps_vE", "ref_gps_vD", ], )
        # downsampled true pos/vel
        self.ref_odo = Sim_data(name="ref_odo", description="true odometer velocity", units=["m/s"],
                legend=["ref_odo"], )
        self.ref_mag = Sim_data(name="ref_mag", description="true magnetic field in the body frame",
                units=["uT", "uT", "uT"], legend=["ref_mag_x", "ref_mag_y", "ref_mag_z"], )
        # sensor measurements
        self.gyro = Sim_data(name="gyro", description="gyro measurements", units=["rad/s", "rad/s", "rad/s"],
                output_units=["deg/s", "deg/s", "deg/s"], legend=["gyro_x", "gyro_y", "gyro_z"], )
        self.accel = Sim_data(name="accel", description="accel measurements", units=["m/s^2", "m/s^2", "m/s^2"],
                legend=["accel_x", "accel_y", "accel_z"], )
        self.gps = Sim_data(name="gps", description="GPS LLA position and NED velocity measurements",
                units=["rad", "rad", "m", "m/s", "m/s", "m/s"], output_units=["deg", "deg", "m", "m/s", "m/s", "m/s"],
                legend=["gps_lat", "gps_lon", "gps_alt", "gps_vN", "gps_vE", "gps_vD"], )
        self.odo = Sim_data(name="odo", description="odometer velocity measurement", units=["m/s"], legend=["odo"], )
        self.mag = Sim_data(name="mag", description="magnetometer measurements", units=["uT", "uT", "uT"],
                legend=["mag_x", "mag_y", "mag_z"], )
        # calibration algorithm output
        self.gyro_cal = Sim_data(name="gyro_cal", description="gyro measurements after factory calibration",
                units=["rad/s", "rad/s", "rad/s"], output_units=["deg/s", "deg/s", "deg/s"],
                legend=["gyro_x", "gyro_y", "gyro_z"], )
        self.accel_cal = Sim_data(name="accel_cal", description="accel measurements after factory calibration",
                units=["m/s^2", "m/s^2", "m/s^2"], legend=["accel_x", "accel_y", "accel_z"], )
        self.mag_cal = Sim_data(name="mag_cal", description="magnetometer measurements after SI&HI calibration",
                units=["uT", "uT", "uT"], legend=["mag_x", "mag_y", "mag_z"], )
        self.soft_iron = Sim_data(name="soft_iron", description="soft iron calibration matrix", plottable=False, )
        self.hard_iron = Sim_data(name="hard_iron", description="hard iron", units=["uT", "uT", "uT", "uT"],
                legend=["offset_x", "offset_y", "offset_z", "radius"], plottable=False, )
        # fusion algorithm output
        self.algo_time = Sim_data(name="algo_time", description="sample time from algo", units=["sec"])
        self.pos = Sim_data(name="pos", description="simulation position from algo", units=["rad", "rad", "m"],
                output_units=["deg", "deg", "m"], legend=["pos_lat", "pos_lon", "pos_alt"], )
        self.vel = Sim_data(name="vel", description="simulation velocity from algo", units=["m/s", "m/s", "m/s"],
                legend=["vel_x", "vel_y", "vel_z"], )
        self.att_quat = Sim_data(name="att_quat", description="simulation attitude (quaternion)  from algo",
                legend=["q0", "q1", "q2", "q3"], )
        self.att_euler = Sim_data(name="att_euler", description="simulation attitude (Euler, ZYX)  from algo",
                units=["rad", "rad", "rad"], output_units=["deg", "deg", "deg"], legend=["Yaw", "Pitch", "Roll"], )
        self.wb = Sim_data(name="wb", description="gyro bias estimation", units=["rad/s", "rad/s", "rad/s"],
                output_units=["deg/s", "deg/s", "deg/s"], legend=["gyro_bias_x", "gyro_bias_y", "gyro_bias_z"], )
        self.ab = Sim_data(name="ab", description="accel bias estimation", units=["m/s^2", "m/s^2", "m/s^2"],
                legend=["accel_bias_x", "accel_bias_y", "accel_bias_z"], )
        self.ad_gyro = Sim_data(name="ad_gyro", description="Allan deviation of gyro",
                units=["rad/s", "rad/s", "rad/s"], output_units=["deg/hr", "deg/hr", "deg/hr"], logx=True, logy=True,
                legend=["AD_wx", "AD_wy", "AD_wz"], )
        self.ad_accel = Sim_data(name="ad_accel", description="Allan deviation of accel",
                units=["m/s^2", "m/s^2", "m/s^2"], logx=True, logy=True, legend=["AD_ax", "AD_ay", "AD_az"], )
        # if using virtual inertial frame
        if self.ref_frame.data == 1:
            # description, position units and legned
            self.ref_pos.description = "true position in the local NED frame"
            self.ref_pos.units = ["m", "m", "m"]
            self.ref_pos.output_units = ["m", "m", "m"]
            self.ref_pos.legend = ["ref_pos_x", "ref_pos_y", "ref_pos_z"]
            self.pos.units = ["m", "m", "m"]
            self.pos.output_units = ["m", "m", "m"]
            self.pos.legend = ["pos_x", "pos_y", "pos_z"]
            # velocity units and legend
            self.ref_vel.legend = ["ref_vel_x", "ref_vel_y", "ref_vel_z"]
            self.vel.legend = ["vel_x", "vel_y", "vel_z"]
            # GPS units and legend
            self.ref_gps.description = ("true GPS position and velocity in the local NED frame")
            self.ref_gps.units = ["m", "m", "m", "m/s", "m/s", "m/s"]
            self.ref_gps.output_units = ["m", "m", "m", "m/s", "m/s", "m/s"]
            self.ref_gps.legend = ["ref_gps_x", "ref_gps_y", "ref_gps_z", "ref_gps_vx", "ref_gps_vy", "ref_gps_vz", ]
            self.gps.description = ("GPS position and velocity measurements in the local NED frame")
            self.gps.units = ["m", "m", "m", "m/s", "m/s", "m/s"]
            self.gps.output_units = ["m", "m", "m", "m/s", "m/s", "m/s"]
            self.gps.legend = ["gps_x", "gps_y", "gps_z", "gps_vx", "gps_vy", "gps_vz"]
        ########## all data ##########
        # __all include all data that may occur in an INS solution.
        self.__all = {
                # error-free data
                self.fs.name: self.fs, self.fs_gps.name: self.fs_gps, self.fs_mag.name: self.fs_mag,
                self.ref_frame.name: self.ref_frame, self.time.name: self.time, self.gps_time.name: self.gps_time,
                self.gps_visibility.name: self.gps_visibility, self.ref_pos.name: self.ref_pos,
                self.ref_vel.name: self.ref_vel, self.ref_att_euler.name: self.ref_att_euler,
                self.ref_att_quat.name: self.ref_att_quat, self.ref_gyro.name: self.ref_gyro,
                self.ref_accel.name: self.ref_accel, self.ref_gps.name: self.ref_gps, self.ref_odo.name: self.ref_odo,
                self.ref_mag.name: self.ref_mag, # sensor data
                self.gyro.name: self.gyro, self.accel.name: self.accel, self.gps.name: self.gps,
                self.odo.name: self.odo, self.mag.name: self.mag, # calibration algorithm output
                self.gyro_cal.name: self.gyro_cal, self.accel_cal.name: self.accel_cal, self.mag_cal.name: self.mag_cal,
                self.soft_iron.name: self.soft_iron, self.hard_iron.name: self.hard_iron, # fusion algorithm output
                self.algo_time.name: self.algo_time, self.pos.name: self.pos, self.vel.name: self.vel,
                self.att_quat.name: self.att_quat, self.att_euler.name: self.att_euler, self.wb.name: self.wb,
                self.ab.name: self.ab, self.ad_gyro.name: self.ad_gyro, self.ad_accel.name: self.ad_accel,
        }
        # all available data that really occur in the simulation.
        self.available = []
        self.available.append(self.ref_frame.name)
        if fs[0] is not None:
            self.fs.data = fs[0]
            self.available.append(self.fs.name)
        else:
            raise ValueError("IMU sampling frequency cannot be None.")
        if fs[1] is not None:
            self.fs_gps.data = fs[1]
            self.available.append(self.fs_gps.name)
        if fs[2] is not None:
            self.fs_mag.data = fs[2]
            self.available.append(self.fs_mag.name)
        # the following will not be saved
        self.__do_not_save = [self.fs.name, self.fs_gps.name, self.fs_mag.name, self.ref_frame.name, ]
        # algorithm output
        self.__algo_output = []
        # associated data mapping. If the user want Euler angles, but only quaternions are
        # available, Euler angles will be automatically calculated, added to self.available and
        # returned.
        self.__data_map = {
                self.ref_att_euler.name: [self.ref_att_quat, self.__euler2quat_zyx],
                self.ref_att_quat.name: [self.ref_att_euler, self.__quat2euler_zyx],
                self.att_euler.name: [self.att_quat, self.__euler2quat_zyx],
                self.att_quat.name: [self.att_euler, self.__quat2euler_zyx],
        }
        # error info, self.get_error_stat() and self.plot() will both update error info
        self.__err = {}

    def add_data(self, data_name, data, key=None, units=None):
        """
        Add data to available.
        Args:
            data_name: data name
            data: a scalar, a numpy array or a dict of the above two. If data is a dict, each
                value in it should be of same type (scalar or numpy array), same size and same
                units.
            key: There are more than one set of data, key is an index of data added this time.
                If key is None, data can be a scalr, a numpy array or a dict of the above two.
                If key is a valid dict key, data can be a scalr or a numpy.
            units: Units of the data. If you know clearly no units convertion is needed, set
                units to None. If you do not know what units are used in the class InsDataMgr,
                you'd better provide the units of the data. Units convertion will be done
                automatically.
                If data is a scalar, units should be a list of one string to define its unit.
                If data is a numpy of size(m,n), units should be a list of n strings
                to define the units.
                If data is a dict, units should be the same as the above two depending on if
                each value in the dict is a scalar or a numpy array.
        """
        if data_name in self.__all:
            self.__all[data_name].add_data(data, key, units)
            if data_name not in self.available:
                self.available.append(data_name)
        else:
            raise ValueError("Unsupported data: %s." % data_name)

    def set_algo_output(self, algo_output):
        """
        Tell data manager what output an algorithm provide
        Args:
            algo_output: a list of data names.
        """
        for i in algo_output:
            if self.is_supported(i):
                self.__algo_output.append(i)
            else:
                raise ValueError("Unsupported algorithm output: %s." % i)

    def get_data(self, data_names):
        """
        Get data section of data_names.
        Args:
            data_names: a list of data names
        Returns:
            data: a list of data corresponding to data_names.
            If there is any unavailable data in data_names, return None
        """
        data = []
        for i in data_names:
            if i in self.available:
                logging.debug(self.__all[i].data)
                data.append(self.__all[i].data)
            else:
                logging.debug("%s is not available." % i)
                return None
        return data

    def get_data_all(self, data_name):
        """
        get the Sim_data object accroding to data_name
        """
        if data_name in self.__all:
            return self.__all[data_name]
        else:
            return None

    def get_error_stats(self, data_name, err_stats_start=0, angle=False, use_output_units=False, extra_opt="", ):
        """
        Get error statistics of data_name.
        Args:
            data_name: name of data whose error will be calculated.
            err_stats_start: This argument specify the starting point in seconds from where
                the error statistics are calculated.
                If it is -1, end-point error statistics will be calculated. In this case,
                the result contains statistics of end-point errors of multiple runs.
                Otherwise, the process error of data specified by data_name starting from
                err_stats_start(in seconds) is calculated. In this case, the result is
                a dict of statistics of process error of each simulatoin run.
                For example, if we want the end-point error of position from a free-integration
                algorithm ran for n times, the result is {'max': numpy.array([rx, ry, rz]),
                                                          'avg': numpy.array([rx, ry, rz]),
                                                          'std': numpy.array([rx, ry, rz])}.
                If we want the process error of an attitude determination algorithm ran for n
                times, the result is {'max': a dict of numpy.array([yaw, pitch, roll]),
                                      'avg': a dict of numpy.array([yaw, pitch, roll]),
                                      'std': a dict of numpy.array([yaw, pitch, roll])}.
            angle: True if this is angle error. Angle error will be converted to be within
                [-pi, pi] before calculating statistics.
            use_output_units: use output units instead of inner units in which the data are
                stored. An automatic unit conversion is done.
            extra_opt: A string option to calculate errors. The following options are supported:
                'ned': NED position error
        Returns:
            err_stat: error statistics.
        """
        # is this set of data available?
        if data_name not in self.available:
            logging.debug("error stats: %s is not available." % data_name)
            return None
        # is the reference of data_name available?
        ref_data_name = "ref_" + data_name
        if ref_data_name not in self.available:
            logging.debug("%s has no reference." % data_name)
            return None
        # calculate error
        err_data_name = "err_" + data_name
        if err_data_name not in self.__err:
            data_err = self.calc_data_err(data_name, ref_data_name, angle, extra_opt)
            if data_err is not None:
                self.__err[data_err.name] = data_err
        if err_stats_start == -1:
            # end-point error
            err_stat = self.__end_point_error_stats(data_name)
        else:
            # process error
            err_stat = self.__process_error_stats(data_name, err_stats_start)
        # unit conversion
        if use_output_units and (err_stat is not None):
            for i in err_stat:
                if isinstance(err_stat[i], dict):
                    for j in err_stat[i]:
                        err_stat[i][j] = convert_unit(err_stat[i][j], self.__err[err_data_name].units,
                                self.__err[err_data_name].output_units, )
                else:
                    err_stat[i] = convert_unit(err_stat[i], self.__err[err_data_name].units,
                            self.__err[err_data_name].output_units, )
        err_stat["units"] = str(self.__err[err_data_name].output_units)
        return err_stat

    def calc_data_err(self, data_name, ref_data_name, angle=False, err_opt=""):
        """
        Calculate error of one set of data.
        Args:
            data_name: name of the intput data
            ref_data_name: name of the reference of the intput data
            angle: True if this is angle error. Angle error will be converted to be within
                [-pi, pi] before calculating statistics.
            err_opt: error options
        Returns:
            an Sim_data object corresponds to data_name
        """
        err = Sim_data(name="err_" + data_name, description="ERROR of " + self.__all[data_name].description,
                units=self.__all[data_name].units, output_units=self.__all[data_name].output_units,
                plottable=self.__all[data_name].plottable, logx=self.__all[data_name].logx,
                logy=self.__all[data_name].logy, grid=self.__all[data_name].grid, legend=self.__all[data_name].legend, )
        # handling position error
        lla = 0
        if data_name == self.pos.name and self.ref_frame.data == 0:
            if err_opt == "ned":
                lla = 1
                err.description = "ERROR of NED position"
                err.units = ["m", "m", "m"]
                err.output_units = ["m", "m", "m"]
                err.legend = ["pos_N", "pos_E", "pos_D"]
            elif err_opt == "ecef":
                lla = 2
                err.description = "ERROR of ECEF position"
                err.units = ["m", "m", "m"]
                err.output_units = ["m", "m", "m"]
                err.legend = ["pos_x", "pos_y", "pos_z"]
        if isinstance(self.__all[data_name].data, dict):
            ref_data = None
            for i in self.__all[data_name].data:
                # get raw reference data for first key in the dict, use reference from last
                # step for other keys to avoid multiple interps.
                if ref_data is None:
                    # use copy to avoid changing data if interp
                    ref_data = self.__all[ref_data_name].data.copy()
                # Interpolation. using ref_data to avoid multiple interps
                if ref_data.shape[0] != self.__all[data_name].data[i].shape[0]:
                    # logging.debug("%s has different number of samples from its reference."% data_name)
                    # logging.debug('Interpolation needed.')
                    if (self.algo_time.name in self.available and self.time.name in self.available):
                        ref_data = self.__interp(self.algo_time.data[i], self.time.data,
                                self.__all[ref_data_name].data, )
                    else:
                        logging.debug("%s or %s is not available." % (self.algo_time.name, self.time.name))
                        return None
                # error stat
                err.data[i] = self.array_error(self.__all[data_name].data[i], ref_data, angle, lla)
        elif isinstance(self.__all[data_name].data, np.ndarray):
            ref_data = self.__all[ref_data_name].data.copy()
            # Interpolation
            if ref_data.shape[0] != self.__all[data_name].data.shape[0]:
                logging.debug("%s has different number of samples from its reference." % data_name)
                logging.debug("Interpolation needed.")
                if (self.algo_time.name in self.available and self.time.name in self.available):
                    ref_data = self.__interp(self.algo_time.data, self.time.data, ref_data)
                else:
                    logging.debug("%s or %s is not available." % (self.algo_time.name, self.time.name))
                    return None
            # error stat
            err.data = self.array_error(self.__all[data_name].data, ref_data, angle, lla)
        return err

    def array_error(self, x, r, angle=False, lla=0):
        """
        Calculate the error of an array w.r.t its reference.
        Args:
            x: input data, numpy array.
            r: reference data, same size as x.
            angle: True if x contains angles, False if not.
        Returns:
            err: error
            :param x:
            :param r:
            :param angle:
            :param lla:
        """
        if lla == 0:
            err = x - r
            if angle:
                for j in range(len(err.flat)):
                    err.flat[j] = angle_range_pi(err.flat[j])
        else:
            # convert x and r to ECEF first
            x_ecef = lla2ecef_batch(x)
            r_ecef = lla2ecef_batch(r)
            err = x_ecef - r_ecef
            # convert ecef err to NED err
            if lla == 1:
                n = err.shape[0]
                for i in range(0, n):
                    c_ne = ecef_to_ned(r[i, 0], r[i, 1])
                    err[i, :] = c_ne.dot(err[i, :])
        return err

    def save_data(self, data_dir):
        """
        save data to files
        Args:
            data_dir: Data files will be saved in data_idr
        Returns:
            data_saved: a list of data that are saved.
        """
        data_saved = []
        for data in self.available:
            if data not in self.__do_not_save:
                # logging.debug('saving %s'% data)
                self.__all[data].save_to_file(data_dir)
                data_saved.append(data)
        return data_saved

    def is_supported(self, data_name):
        """
        Tell if this set of data is supported or not
        """
        return data_name in self.__all.keys()

    def is_available(self, data_name, key=None):
        """
        Tell if data_name is available. If key is not None, further tell if data in data_name
        has the key.
        """
        # if data_name is available
        rtn = data_name in self.available
        # if data_name has key
        if rtn and key is not None:
            if isinstance(self.__all[data_name].data, dict):
                rtn = key in self.__all[data_name].data.keys()
            else:
                rtn = False
        return rtn

    def __end_point_error_stats(self, data_name, group=True):
        """
        end-point error statistics
        """
        # error available?
        err_data_name = "err_" + data_name
        if err_data_name not in self.__err:
            logging.debug("__end_point_error_stats: %s is not available." % data_name)
        # collect data according to keys
        if isinstance(self.__err[err_data_name].data, dict):
            # collect groups
            groups = None
            if group:
                keys = self.__err[err_data_name].data.keys()
                groups = self.__get_data_groups(keys)
            # only one group
            if groups is None:
                # a dict contains data of multiple runs
                err = []
                for i in self.__err[err_data_name].data:
                    err.append(self.__err[err_data_name].data[i][-1, :])
                # convert list to np.array
                err = np.array(err)
                return self.__array_stats(err)
            # at least two groups
            else:
                stat = {"max": {}, "avg": {}, "std": {}}
                for j in groups:
                    err = []
                    for i in self.__err[err_data_name].data:
                        if j in i:
                            err.append(self.__err[err_data_name].data[i][-1, :])
                    tmp = self.__array_stats(err)
                    stat["max"][j] = tmp["max"]
                    stat["avg"][j] = tmp["avg"]
                    stat["std"][j] = tmp["std"]
                return stat
        elif isinstance(self.__err[err_data_name].data, np.ndarray):
            err = self.__err[err_data_name].data[-1, :]
            return self.__array_stats(err)
        else:
            logging.debug("Unsupported data type to calculate error statitics for %s" % data_name)
            return None

    def __process_error_stats(self, data_name, err_stats_start):
        """
        process error statistics
        """
        # error available?
        err_data_name = "err_" + data_name
        if err_data_name not in self.__err:
            logging.debug("__process_error_stats: %s is not available." % data_name)
        # begin to calculate error stat
        if isinstance(self.__all[data_name].data, dict):
            stat = {"max": {}, "avg": {}, "std": {}}
            for i in self.__all[data_name].data:
                # error stat
                if i in self.algo_time.data:
                    idx = np.where(self.algo_time.data[i] >= err_stats_start)[0]
                else:
                    idx = np.where(self.time.data >= err_stats_start)[0]
                if idx.shape[0] == 0:
                    logging.debug("err_stats_start exceeds max data points.")
                    idx = 0
                else:
                    idx = idx[0]
                err = self.__err[err_data_name].data[i][idx::, :]
                tmp = self.__array_stats(err)
                stat["max"][i] = tmp["max"]
                stat["avg"][i] = tmp["avg"]
                stat["std"][i] = tmp["std"]
            return stat
        elif isinstance(self.__all[data_name].data, np.ndarray):
            # error stat
            err = self.__err[err_data_name].data
            return self.__array_stats(err)
        else:
            logging.debug("Unsupported data type to calculate error statitics for %s" % data_name)
            return None

    def __array_stats(self, x):
        """
        statistics of array x.
        Args:
            x is a numpy array of size (m,n) or (m,). m is number of sample. n is its dimension.
        Returns:
            {'max':, 'avg':, 'std': }
        """
        # statistics
        return {
                "max": np.max(np.abs(x), 0), "avg": np.average(x, 0), "std": np.std(x, 0),
        }

    def __get_data_groups(self, keys):
        """
        Check if the keys can be grouped. The key should be named as GROUP_idx.
        For example list of keys [algo0_0, algo0_1 algo1_0, algo1_1] can be divided
        into two groups: [algo0, algo1], and each group contains two elements.
        This is used to calculate statistics of error of results from multiple algorithms.
        Args:
            keys: dict keys
        Return:
            a list of groups if there is more than one group, and none if there is only one group
        """
        groups = []
        for i in keys:
            idx = str(i).rfind("_")
            if idx == -1:
                groups = []  # one of the keys cannot be grouped, do not group
                break
            group_name = i[0:idx]
            if group_name not in groups:
                groups.append(group_name)
        if len(groups) <= 1:
            groups = None
        return groups

    def __interp(self, x, xp, fp):
        """
        data interpolation
        """
        m = x.shape[0]
        ndim = fp.ndim
        if ndim == 1:
            return np.interp(x, xp, fp)
        elif ndim == 2:
            y = np.zeros((m, fp.shape[1]))
            for i in range(fp.shape[1]):
                y[:, i] = np.interp(x, xp, fp[:, i])
            return y
        else:
            raise ValueError("only 1-D or 2-D fp is supported.")

    def __quat2euler_zyx(self, src, dst):
        """
        quaternion to Euler angles (zyx)
        """
        if isinstance(src.data, np.ndarray):
            n = src.data.shape[0]
            dst.data = np.zeros((n, 3))
            for j in range(n):
                dst.data[j, :] = quat2euler(src.data[j, :])
        elif isinstance(src.data, dict):
            for i in src.data:
                n = src.data[i].shape[0]
                euler = np.zeros((n, 3))
                for j in range(n):
                    euler[j, :] = quat2euler(src.data[i][j, :])
                dst.data[i] = euler
        else:
            raise ValueError("%s is not a dict or numpy array." % src.name)

    def __euler2quat_zyx(self, src, dst):
        """
        Euler angles (zyx) to quaternion
        """
        # array
        if isinstance(src.data, np.ndarray):
            n = src.data.shape[0]
            dst.data = np.zeros((n, 4))
            for j in range(n):
                dst.data[j, :] = euler2quat(src.data[j, :])
        # dict
        elif isinstance(src.data, dict):
            for i in src.data:
                n = src.data[i].shape[0]
                quat = np.zeros((n, 4))
                for j in range(n):
                    quat[j, :] = euler2quat(src.data[i][j, :])
                dst.data[i] = quat
        else:
            raise ValueError("%s is not a dict or numpy array." % src.name)


class InsAlgoMgr(object):
    """
    A class that manages all algorithms in an INS solution.
    """

    def __init__(self, algo):
        """
        algo: a user defined algorithm, or a list of that.
        """
        if algo is None:
            self.algo = None
        else:
            # self.algo is a list of algorithms
            if isinstance(algo, list):
                self.algo = algo
            else:
                self.algo = [algo]
        self.input = []
        self.output = []
        self.nin = 0
        self.nout = 0
        self.nalgo = 0
        self.input_alloc = []
        self.output_alloc = []
        # check algorithm
        if self.algo is not None:
            self.__check_algo()

    def run_algo(self, input_data, keys=None):
        """
        Run the algorithm with given input
        Args:
            input_data: algorithm input. It is a list of data defined in self.input. Each element
                in the list can be a scalar, a numpy array or a dict of the above two.
                Scalars and numpy arrays stays the same for each run of the algorithm.
                Data in dict are chosen according to keys for each run of the algorithm.
            keys: a list of keys to index data in dicts in input. For example, we have multiple
                sets of gyro data: w={key0: set_of_data_#0, key1: set_of_data#1}.
                w is a element of input. keys should be [key0, key1]. For each run of the algo,
                gyro data is chosen accroding to the keys.
        Returns:
            results: a list containing data defined in self.output.  Each output in results is
                a dict with keys 'algorithm_name' + '_' + 'simulation run'. For example:
                algo0_0, algo0_1, algo1_0, algo1_1, ......
        """
        if len(input_data) != self.nin:
            raise ValueError("Required %s input, but provide %s." % (self.nin, len(input_data)))
        #### call the algorithm
        # run the algorithm once
        results = []
        for i in range(self.nout):
            results.append({})
        # generate keys to run simulation
        if keys is None:
            keys = [0]  # defauly, only run once
            # if there are multiple simulations runs
            for i in input_data:
                if isinstance(i, dict):
                    keys = list(i.keys())
                    break
        # run each algorithm
        for i in range(self.nalgo):
            # algo name will be used as a key to index results of this algo
            this_algo_name = self.get_algo_name(i)
            # run the algorithm for each simulation
            for key in keys:
                self.algo[i].reset()  # reset/initialize before each run
                # prepare input
                set_of_input = []
                for j in self.input_alloc[i]:  # j is the index of input of this algo in self.input
                    if isinstance(input_data[j], dict):
                        if key in input_data[j]:
                            set_of_input.append(input_data[j][key])
                        else:
                            raise ValueError(
                                    "set_of_input has keys %s, but you are requiring %s" % (input_data[j].keys(), key))
                    else:
                        set_of_input.append(input_data[j])
                self.algo[i].run(copy.deepcopy(set_of_input))  # deepcopy to avoid being changed
                # get algorithm output of this run
                this_results = self.algo[i].get_results()
                # add algorithm output of this run to results
                for j in range(len(self.output_alloc[i])):
                    t = results[self.output_alloc[i][j]]
                    t[this_algo_name + "_" + str(key)] = this_results[j]
        return t

    def get_algo_name(self, i):
        """
        get the name of the i-th algo
        Args:
            i: index of the algorithm
        Returns:
            name of the i-th algo, None if there is no i-th algo
        """
        if self.algo is None:
            return None
        elif i >= self.nalgo:
            return None
        else:
            if hasattr(self.algo[i], "name"):
                return self.algo[i].name
            else:
                return "algo" + str(i)

    def __check_algo(self):
        """
        Generate expressions to handle algorithm input and output.

        Returns:
            Raise ValueError if algorithm has no input or output;
            Raise ValueError if algorithm input and output have unsupported elements
            Raise TypeError if algorithm input or output is not a list or tuple
        """
        # check if all algorithms has at least one input and at least one output
        try:
            for algo in self.algo:
                if len(algo.input) < 1 or len(algo.output) < 1:
                    raise ValueError
        except:
            raise ValueError("algorithm input or output is not a valid list or tuple.")
        # get input for all algoriths
        for algo in self.algo:
            self.input = list(set(self.input).union(set(algo.input)))
            self.output = list(set(self.output).union(set(algo.output)))
        # how to allocate input/output to each algorithm
        for algo in self.algo:
            idx = []
            for i in algo.input:
                idx.append(self.input.index(i))
            self.input_alloc.append(idx)
            idx = []
            for i in algo.output:
                idx.append(self.output.index(i))
            self.output_alloc.append(idx)
        self.nin = len(self.input)
        self.nout = len(self.output)
        self.nalgo = len(self.algo)


def get_cn2b_acc_mag_ned(acc, mag):
    """
    Calculate NED to body transformation matrix from acc and mag.
    Args:
        acc: acc measurement, numpy array of 3x1.
        mag: mag measurement, numpy array of 3x1.
    Returns:
        cn2b: transformation matrix from NED to body
    """
    z = -acc / math.sqrt(np.dot(acc, acc))
    acc_cross_mag = np.cross(z, mag)
    y = acc_cross_mag / math.sqrt(np.dot(acc_cross_mag, acc_cross_mag))
    x = np.cross(y, z)
    cn2b = np.zeros((3, 3))
    cn2b[0][0] = x[0]
    cn2b[1][0] = x[1]
    cn2b[2][0] = x[2]
    cn2b[0][1] = y[0]
    cn2b[1][1] = y[1]
    cn2b[2][1] = y[2]
    cn2b[0][2] = z[0]
    cn2b[1][2] = z[1]
    cn2b[2][2] = z[2]
    return cn2b


def quat_normalize(q):
    """
    Normalize a quaternion, scalar part is always non-negative
    Args:
        q: quaternion
    Returns:
        qn: normalized quaternion, scalar part is always non-negative
    """
    if q[0] < 0:
        q = -q
    q_norm = math.sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3])
    qn = q / q_norm
    return qn


def quat_conj(q):
    """
    Conjugate of quaternion
    Args:
        q: quaternion, scalar first
    Returns:
        qc: quaternion conjugate
    """
    qc = q
    qc[1] = -q[1]
    qc[2] = -q[2]
    qc[3] = -q[3]


def quat_multiply(q1, q2):
    """
    Multiplication of two quaternions
    Args:
        q1: quaternion, scalar first
        q2: quaternion, scalar first
    Returns:
        q = q1 * q2
    """
    q = np.array([0.0, 0.0, 0.0, 0.0])
    q[0] = q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] - q1[3] * q2[3]
    q[1] = q1[0] * q2[1] + q1[1] * q2[0] + q1[2] * q2[3] - q1[3] * q2[2]
    q[2] = q1[0] * q2[2] - q1[1] * q2[3] + q1[2] * q2[0] + q1[3] * q2[1]
    q[3] = q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1] + q1[3] * q2[0]
    return q


def quat2euler(q, rot_seq="zyx"):
    """
    Convert quaternion to Euler angles
    Args:
        q: quaternion, [q0, q1, q2, q3], q0 is the scalar
        rot_seq: rotation sequence corresponding to the angles.
    Return:
        angles: 3x1 Euler angles, rad.
    """
    rot_seq = rot_seq.lower()
    if rot_seq == "zyx":
        [r1, r2, r3] = three_axis_rot(2.0 * (q[1] * q[2] + q[0] * q[3]),
                                      q[0] ** 2.0 + q[1] ** 2.0 - q[2] ** 2.0 - q[3] ** 2.0,
                                      -2.0 * (q[1] * q[3] - q[0] * q[2]), 2.0 * (q[2] * q[3] + q[0] * q[1]),
                                      q[0] ** 2.0 - q[1] ** 2.0 - q[2] ** 2.0 + q[3] ** 2.0, )
        return np.array([r1, r2, r3])
    elif rot_seq == "zyz":
        [r1, r2, r3] = two_axis_rot(2.0 * (q[2] * q[3] - q[0] * q[1]), 2.0 * (q[1] * q[3] + q[0] * q[2]),
                                    q[0] ** 2.0 - q[1] ** 2.0 - q[2] ** 2.0 + q[3] ** 2.0,
                                    2.0 * (q[2] * q[3] + q[0] * q[1]), -2.0 * (q[1] * q[3] - q[0] * q[2]), )
        return np.array([r1, r2, r3])
    elif rot_seq == "zxy":
        [r1, r2, r3] = three_axis_rot(-2.0 * (q[1] * q[2] - q[0] * q[3]),
                                      q[0] ** 2.0 - q[1] ** 2.0 + q[2] ** 2.0 - q[3] ** 2.0,
                                      2.0 * (q[2] * q[3] + q[0] * q[1]), -2.0 * (q[1] * q[3] - q[0] * q[2]),
                                      q[0] ** 2.0 - q[1] ** 2.0 - q[2] ** 2.0 + q[3] ** 2.0, )
        return np.array([r1, r2, r3])
    elif rot_seq == "zxz":
        [r1, r2, r3] = two_axis_rot(2.0 * (q[1] * q[3] + q[0] * q[2]), -2.0 * (q[2] * q[3] - q[0] * q[1]),
                                    q[0] ** 2.0 - q[1] ** 2.0 - q[2] ** 2.0 + q[3] ** 2.0,
                                    2.0 * (q[1] * q[3] - q[0] * q[2]), 2.0 * (q[2] * q[3] + q[0] * q[1]), )
        return np.array([r1, r2, r3])
    elif rot_seq == "yxz":
        [r1, r2, r3] = three_axis_rot(2.0 * (q[1] * q[3] + q[0] * q[2]),
                                      q[0] ** 2.0 - q[1] ** 2.0 - q[2] ** 2.0 + q[3] ** 2.0,
                                      -2.0 * (q[2] * q[3] - q[0] * q[1]), 2.0 * (q[1] * q[2] + q[0] * q[3]),
                                      q[0] ** 2.0 - q[1] ** 2.0 + q[2] ** 2.0 - q[3] ** 2.0, )
        return np.array([r1, r2, r3])
    elif rot_seq == "yxy":
        [r1, r2, r3] = two_axis_rot(2.0 * (q[1] * q[2] - q[0] * q[3]), 2.0 * (q[2] * q[3] + q[0] * q[1]),
                                    q[0] ** 2.0 - q[1] ** 2.0 + q[2] ** 2.0 - q[3] ** 2.0,
                                    2.0 * (q[1] * q[2] + q[0] * q[3]), -2.0 * (q[2] * q[3] - q[0] * q[1]), )
        return np.array([r1, r2, r3])
    elif rot_seq == "yzx":
        [r1, r2, r3] = three_axis_rot(-2.0 * (q[1] * q[3] - q[0] * q[2]),
                                      q[0] ** 2.0 + q[1] ** 2.0 - q[2] ** 2.0 - q[3] ** 2.0,
                                      2.0 * (q[1] * q[2] + q[0] * q[3]), -2.0 * (q[2] * q[3] - q[0] * q[1]),
                                      q[0] ** 2.0 - q[1] ** 2.0 + q[2] ** 2.0 - q[3] ** 2.0, )
        return np.array([r1, r2, r3])
    elif rot_seq == "yzy":
        [r1, r2, r3] = two_axis_rot(2.0 * (q[2] * q[3] + q[0] * q[1]), -2.0 * (q[1] * q[2] - q[0] * q[3]),
                                    q[0] ** 2.0 - q[1] ** 2.0 + q[2] ** 2.0 - q[3] ** 2.0,
                                    2.0 * (q[2] * q[3] - q[0] * q[1]), 2.0 * (q[1] * q[2] + q[0] * q[3]), )
        return np.array([r1, r2, r3])
    elif rot_seq == "xyz":
        [r1, r2, r3] = three_axis_rot(-2.0 * (q[2] * q[3] - q[0] * q[1]),
                                      q[0] ** 2.0 - q[1] ** 2.0 - q[2] ** 2.0 + q[3] ** 2.0,
                                      2.0 * (q[1] * q[3] + q[0] * q[2]), -2.0 * (q[1] * q[2] - q[0] * q[3]),
                                      q[0] ** 2.0 + q[1] ** 2.0 - q[2] ** 2.0 - q[3] ** 2.0, )
        return np.array([r1, r2, r3])
    elif rot_seq == "xyx":
        [r1, r2, r3] = two_axis_rot(2.0 * (q[1] * q[2] + q[0] * q[3]), -2.0 * (q[1] * q[3] - q[0] * q[2]),
                                    q[0] ** 2.0 + q[1] ** 2.0 - q[2] ** 2.0 - q[3] ** 2.0,
                                    2.0 * (q[1] * q[2] - q[0] * q[3]), 2.0 * (q[1] * q[3] + q[0] * q[2]), )
        return np.array([r1, r2, r3])
    elif rot_seq == "xzy":
        [r1, r2, r3] = three_axis_rot(2.0 * (q[2] * q[3] + q[0] * q[1]),
                                      q[0] ** 2.0 - q[1] ** 2.0 + q[2] ** 2.0 - q[3] ** 2.0,
                                      -2.0 * (q[1] * q[2] - q[0] * q[3]), 2.0 * (q[1] * q[3] + q[0] * q[2]),
                                      q[0] ** 2.0 + q[1] ** 2.0 - q[2] ** 2.0 - q[3] ** 2.0, )
        return np.array([r1, r2, r3])
    elif rot_seq == "xzx":
        [r1, r2, r3] = two_axis_rot(2.0 * (q[1] * q[3] - q[0] * q[2]), 2.0 * (q[1] * q[2] + q[0] * q[3]),
                                    q[0] ** 2.0 + q[1] ** 2.0 - q[2] ** 2.0 - q[3] ** 2.0,
                                    2.0 * (q[1] * q[3] + q[0] * q[2]), -2.0 * (q[1] * q[2] - q[0] * q[3]), )
        return np.array([r1, r2, r3])
    else:
        return False


def euler2quat(angles, rot_seq="zyx"):
    """
    Convert Euler angles to quaternion.
    Args:
        angles: 3x1 Euler angles, rad.
        rot_seq: rotation sequence corresponding to the angles.
    Return:
        q: quaternion, [q0, q1, q2, q3], q0 is the scalar
    """
    cangle = np.cos(0.5 * angles)
    sangle = np.sin(0.5 * angles)
    rot_seq = rot_seq.lower()
    if rot_seq == "zyx":
        return np.array([cangle[0] * cangle[1] * cangle[2] + sangle[0] * sangle[1] * sangle[2],
                         cangle[0] * cangle[1] * sangle[2] - sangle[0] * sangle[1] * cangle[2],
                         cangle[0] * sangle[1] * cangle[2] + sangle[0] * cangle[1] * sangle[2],
                         sangle[0] * cangle[1] * cangle[2] - cangle[0] * sangle[1] * sangle[2], ])
    elif rot_seq == "zyz":
        return np.array([cangle[0] * cangle[1] * cangle[2] - sangle[0] * cangle[1] * sangle[2],
                         cangle[0] * sangle[1] * sangle[2] - sangle[0] * sangle[1] * cangle[2],
                         cangle[0] * sangle[1] * cangle[2] + sangle[0] * sangle[1] * sangle[2],
                         sangle[0] * cangle[1] * cangle[2] + cangle[0] * cangle[1] * sangle[2], ])
    elif rot_seq == "zxy":
        return np.array([cangle[0] * cangle[1] * cangle[2] - sangle[0] * sangle[1] * sangle[2],
                         cangle[0] * sangle[1] * cangle[2] - sangle[0] * cangle[1] * sangle[2],
                         cangle[0] * cangle[1] * sangle[2] + sangle[0] * sangle[1] * cangle[2],
                         cangle[0] * sangle[1] * sangle[2] + sangle[0] * cangle[1] * cangle[2], ])
    elif rot_seq == "zxz":
        return np.array([cangle[0] * cangle[1] * cangle[2] - sangle[0] * cangle[1] * sangle[2],
                         cangle[0] * sangle[1] * cangle[2] + sangle[0] * sangle[1] * sangle[2],
                         sangle[0] * sangle[1] * cangle[2] - cangle[0] * sangle[1] * sangle[2],
                         cangle[0] * cangle[1] * sangle[2] + sangle[0] * cangle[1] * cangle[2], ])
    elif rot_seq == "yxz":
        return np.array([cangle[0] * cangle[1] * cangle[2] + sangle[0] * sangle[1] * sangle[2],
                         cangle[0] * sangle[1] * cangle[2] + sangle[0] * cangle[1] * sangle[2],
                         sangle[0] * cangle[1] * cangle[2] - cangle[0] * sangle[1] * sangle[2],
                         cangle[0] * cangle[1] * sangle[2] - sangle[0] * sangle[1] * cangle[2], ])
    elif rot_seq == "yxy":
        return np.array([cangle[0] * cangle[1] * cangle[2] - sangle[0] * cangle[1] * sangle[2],
                         cangle[0] * sangle[1] * cangle[2] + sangle[0] * sangle[1] * sangle[2],
                         sangle[0] * cangle[1] * cangle[2] + cangle[0] * cangle[1] * sangle[2],
                         cangle[0] * sangle[1] * sangle[2] - sangle[0] * sangle[1] * cangle[2], ])
    elif rot_seq == "yzx":
        return np.array([cangle[0] * cangle[1] * cangle[2] - sangle[0] * sangle[1] * sangle[2],
                         cangle[0] * cangle[1] * sangle[2] + sangle[0] * sangle[1] * cangle[2],
                         cangle[0] * sangle[1] * sangle[2] + sangle[0] * cangle[1] * cangle[2],
                         cangle[0] * sangle[1] * cangle[2] - sangle[0] * cangle[1] * sangle[2], ])
    elif rot_seq == "yzy":
        return np.array([cangle[0] * cangle[1] * cangle[2] - sangle[0] * cangle[1] * sangle[2],
                         sangle[0] * sangle[1] * cangle[2] - cangle[0] * sangle[1] * sangle[2],
                         cangle[0] * cangle[1] * sangle[2] + sangle[0] * cangle[1] * cangle[2],
                         cangle[0] * sangle[1] * cangle[2] + sangle[0] * sangle[1] * sangle[2], ])
    elif rot_seq == "xyz":
        return np.array([cangle[0] * cangle[1] * cangle[2] - sangle[0] * sangle[1] * sangle[2],
                         cangle[0] * sangle[1] * sangle[2] + sangle[0] * cangle[1] * cangle[2],
                         cangle[0] * sangle[1] * cangle[2] - sangle[0] * cangle[1] * sangle[2],
                         cangle[0] * cangle[1] * sangle[2] + sangle[0] * sangle[1] * cangle[2], ])
    elif rot_seq == "xyx":
        return np.array([cangle[0] * cangle[1] * cangle[2] - sangle[0] * cangle[1] * sangle[2],
                         cangle[0] * cangle[1] * sangle[2] + sangle[0] * cangle[1] * cangle[2],
                         cangle[0] * sangle[1] * cangle[2] + sangle[0] * sangle[1] * sangle[2],
                         sangle[0] * sangle[1] * cangle[2] - cangle[0] * sangle[1] * sangle[2], ])
    elif rot_seq == "xzy":
        return np.array([cangle[0] * cangle[1] * cangle[2] + sangle[0] * sangle[1] * sangle[2],
                         sangle[0] * cangle[1] * cangle[2] - cangle[0] * sangle[1] * sangle[2],
                         cangle[0] * cangle[1] * sangle[2] - sangle[0] * sangle[1] * cangle[2],
                         cangle[0] * sangle[1] * cangle[2] + sangle[0] * cangle[1] * sangle[2], ])
    elif rot_seq == "xzx":
        return np.array([cangle[0] * cangle[1] * cangle[2] - sangle[0] * cangle[1] * sangle[2],
                         cangle[0] * cangle[1] * sangle[2] + sangle[0] * cangle[1] * cangle[2],
                         cangle[0] * sangle[1] * sangle[2] - sangle[0] * sangle[1] * cangle[2],
                         cangle[0] * sangle[1] * cangle[2] + sangle[0] * sangle[1] * sangle[2], ])
    else:
        return False


def quat2dcm(q):
    """
    Convert quaternion to direction cosine matrix
    Args:
        q: quaternion, [q0, q1, q2, q3], q0 is the scalar
    Return:
        dcm: direction cosine matrix
    """
    q0q0 = q[0] * q[0]
    q0q1 = q[0] * q[1]
    q0q2 = q[0] * q[2]
    q0q3 = q[0] * q[3]
    q1q1 = q[1] * q[1]
    q1q2 = q[1] * q[2]
    q1q3 = q[1] * q[3]
    q2q2 = q[2] * q[2]
    q2q3 = q[2] * q[3]
    q3q3 = q[3] * q[3]
    dcm = np.zeros((3, 3))
    dcm[0, 0] = q0q0 + q1q1 - q2q2 - q3q3
    dcm[0, 1] = 2.0 * (q1q2 + q0q3)
    dcm[0, 2] = 2.0 * (q1q3 - q0q2)
    dcm[1, 0] = 2.0 * (q1q2 - q0q3)
    dcm[1, 1] = q0q0 - q1q1 + q2q2 - q3q3
    dcm[1, 2] = 2.0 * (q2q3 + q0q1)
    dcm[2, 0] = 2.0 * (q1q3 + q0q2)
    dcm[2, 1] = 2.0 * (q2q3 - q0q1)
    dcm[2, 2] = q0q0 - q1q1 - q2q2 + q3q3
    return dcm


def dcm2quat(c):
    """
    Convert direction cosine matrix to quaternion
    Args:
        c: direciton cosine matrix
    Returns:
        q: quaternion, scalar first
    """
    tr = c[0, 0] + c[1, 1] + c[2, 2]
    tmp = np.array([0.0, 0.0, 0.0, 0.0])
    if tr > 0.0:
        tmp[0] = 0.5 * math.sqrt(1.0 + tr)
        tmp[1] = 0.25 / tmp[0] * (c[1, 2] - c[2, 1])
        tmp[2] = 0.25 / tmp[0] * (c[2, 0] - c[0, 2])
        tmp[3] = 0.25 / tmp[0] * (c[0, 1] - c[1, 0])
    else:
        if (c[1, 1] > c[0, 0]) and (c[1, 1] > c[2, 2]):
            sqdip1 = math.sqrt(c[1, 1] - c[0, 0] - c[2, 2] + 1.0)
            tmp[2] = 0.5 * sqdip1
            if sqdip1 != 0.0:  # // if it equals 0, something is wrong
                sqdip1 = 0.5 / sqdip1
            tmp[0] = (c[2, 0] - c[0, 2]) * sqdip1
            tmp[1] = (c[0, 1] + c[1, 0]) * sqdip1
            tmp[3] = (c[1, 2] + c[2, 1]) * sqdip1
        elif c[2, 2] > c[0, 0]:
            sqdip1 = math.sqrt(c[2, 2] - c[0, 0] - c[1, 1] + 1.0)
            tmp[3] = 0.5 * sqdip1
            if sqdip1 != 0.0:  # // if it equals 0, something is wrong
                sqdip1 = 0.5 / sqdip1
            tmp[0] = (c[0, 1] - c[1, 0]) * sqdip1
            tmp[1] = (c[2, 0] + c[0, 2]) * sqdip1
            tmp[2] = (c[1, 2] + c[2, 1]) * sqdip1
        else:
            sqdip1 = math.sqrt(c[0, 0] - c[1, 1] - c[2, 2] + 1.0)
            tmp[1] = 0.5 * sqdip1
            if sqdip1 != 0.0:  # // if it equals 0, something is wrong
                sqdip1 = 0.5 / sqdip1
            tmp[0] = (c[1, 2] - c[2, 1]) * sqdip1
            tmp[2] = (c[0, 1] + c[1, 0]) * sqdip1
            tmp[3] = (c[2, 0] + c[0, 2]) * sqdip1
    # quaternion normalization, *** no need if dcm is really a dcm
    # quatNormalize(tmp,q)
    # ensure q[0] is non-negative
    if tmp[0] < 0:
        q = -1.0 * tmp
    else:
        q = tmp
    return q


def euler2dcm(angles, rot_seq="zyx"):
    """
    Convert Euler angles to direction cosine matrix.
    The Euler angles rotate the frame n to the frame b according to specified
    rotation sequency. The DCM is a 3x3 coordinate transformation matrix from n
    to b. That is v_b  = DCM * v_n. '_b' or '_n' mean the vector 'v' is expressed
    in the frame b or n.
    Args:
        angles: 3x1 Euler angles, rad.
        rot_seq: rotation sequence corresponding to the angles.
    Returns:
        dcm: 3x3 coordinate transformation matrix from n to b
    """
    dcm = np.zeros((3, 3))
    cangle = np.cos(angles)
    sangle = np.sin(angles)
    rot_seq = rot_seq.lower()
    if rot_seq == "zyx":
        dcm[0, 0] = cangle[1] * cangle[0]
        dcm[0, 1] = cangle[1] * sangle[0]
        dcm[0, 2] = -sangle[1]
        dcm[1, 0] = sangle[2] * sangle[1] * cangle[0] - cangle[2] * sangle[0]
        dcm[1, 1] = sangle[2] * sangle[1] * sangle[0] + cangle[2] * cangle[0]
        dcm[1, 2] = cangle[1] * sangle[2]
        dcm[2, 0] = sangle[1] * cangle[2] * cangle[0] + sangle[0] * sangle[2]
        dcm[2, 1] = sangle[1] * cangle[2] * sangle[0] - cangle[0] * sangle[2]
        dcm[2, 2] = cangle[1] * cangle[2]
        return dcm
    elif rot_seq == "zyz":
        dcm[0, 0] = cangle[0] * cangle[2] * cangle[1] - sangle[0] * sangle[2]
        dcm[0, 1] = sangle[0] * cangle[2] * cangle[1] + cangle[0] * sangle[2]
        dcm[0, 2] = -sangle[1] * cangle[2]
        dcm[1, 0] = -cangle[0] * cangle[1] * sangle[2] - sangle[0] * cangle[2]
        dcm[1, 1] = -sangle[0] * cangle[1] * sangle[2] + cangle[0] * cangle[2]
        dcm[1, 2] = sangle[1] * sangle[2]
        dcm[2, 0] = cangle[0] * sangle[1]
        dcm[2, 1] = sangle[0] * sangle[1]
        dcm[2, 2] = cangle[1]
        return dcm
    elif rot_seq == "zxy":
        dcm[0, 0] = cangle[2] * cangle[0] - sangle[1] * sangle[2] * sangle[0]
        dcm[0, 1] = cangle[2] * sangle[0] + sangle[1] * sangle[2] * cangle[0]
        dcm[0, 2] = -sangle[2] * cangle[1]
        dcm[1, 0] = -cangle[1] * sangle[0]
        dcm[1, 1] = cangle[1] * cangle[0]
        dcm[1, 2] = sangle[1]
        dcm[2, 0] = sangle[2] * cangle[0] + sangle[1] * cangle[2] * sangle[0]
        dcm[2, 1] = sangle[2] * sangle[0] - sangle[1] * cangle[2] * cangle[0]
        dcm[2, 2] = cangle[1] * cangle[2]
        return dcm
    elif rot_seq == "zxz":
        dcm[0, 0] = -sangle[0] * cangle[1] * sangle[2] + cangle[0] * cangle[2]
        dcm[0, 1] = cangle[0] * cangle[1] * sangle[2] + sangle[0] * cangle[2]
        dcm[0, 2] = sangle[1] * sangle[2]
        dcm[1, 0] = -sangle[0] * cangle[2] * cangle[1] - cangle[0] * sangle[2]
        dcm[1, 1] = cangle[0] * cangle[2] * cangle[1] - sangle[0] * sangle[2]
        dcm[1, 2] = sangle[1] * cangle[2]
        dcm[2, 0] = sangle[0] * sangle[1]
        dcm[2, 1] = -cangle[0] * sangle[1]
        dcm[2, 2] = cangle[1]
        return dcm
    elif rot_seq == "yxz":
        dcm[0, 0] = cangle[0] * cangle[2] + sangle[1] * sangle[0] * sangle[2]
        dcm[0, 1] = cangle[1] * sangle[2]
        dcm[0, 2] = -sangle[0] * cangle[2] + sangle[1] * cangle[0] * sangle[2]
        dcm[1, 0] = -cangle[0] * sangle[2] + sangle[1] * sangle[0] * cangle[2]
        dcm[1, 1] = cangle[1] * cangle[2]
        dcm[1, 2] = sangle[0] * sangle[2] + sangle[1] * cangle[0] * cangle[2]
        dcm[2, 0] = sangle[0] * cangle[1]
        dcm[2, 1] = -sangle[1]
        dcm[2, 2] = cangle[1] * cangle[0]
        return dcm
    elif rot_seq == "yxy":
        dcm[0, 0] = -sangle[0] * cangle[1] * sangle[2] + cangle[0] * cangle[2]
        dcm[0, 1] = sangle[1] * sangle[2]
        dcm[0, 2] = -cangle[0] * cangle[1] * sangle[2] - sangle[0] * cangle[2]
        dcm[1, 0] = sangle[0] * sangle[1]
        dcm[1, 1] = cangle[1]
        dcm[1, 2] = cangle[0] * sangle[1]
        dcm[2, 0] = sangle[0] * cangle[2] * cangle[1] + cangle[0] * sangle[2]
        dcm[2, 1] = -sangle[1] * cangle[2]
        dcm[2, 2] = cangle[0] * cangle[2] * cangle[1] - sangle[0] * sangle[2]
        return dcm
    elif rot_seq == "yxz":
        dcm[0, 0] = cangle[0] * cangle[1]
        dcm[0, 1] = sangle[1]
        dcm[0, 2] = -sangle[0] * cangle[1]
        dcm[1, 0] = -cangle[2] * cangle[0] * sangle[1] + sangle[2] * sangle[0]
        dcm[1, 1] = cangle[1] * cangle[2]
        dcm[1, 2] = cangle[2] * sangle[0] * sangle[1] + sangle[2] * cangle[0]
        dcm[2, 0] = sangle[2] * cangle[0] * sangle[1] + cangle[2] * sangle[0]
        dcm[2, 1] = -sangle[2] * cangle[1]
        dcm[2, 2] = -sangle[2] * sangle[0] * sangle[1] + cangle[2] * cangle[0]
        return dcm
    elif rot_seq == "yzy":
        dcm[0, 0] = cangle[0] * cangle[2] * cangle[1] - sangle[0] * sangle[2]
        dcm[0, 1] = sangle[1] * cangle[2]
        dcm[0, 2] = -sangle[0] * cangle[2] * cangle[1] - cangle[0] * sangle[2]
        dcm[1, 0] = -cangle[0] * sangle[1]
        dcm[1, 1] = cangle[1]
        dcm[1, 2] = sangle[0] * sangle[1]
        dcm[2, 0] = cangle[0] * cangle[1] * sangle[2] + sangle[0] * cangle[2]
        dcm[2, 1] = sangle[1] * sangle[2]
        dcm[2, 2] = -sangle[0] * cangle[1] * sangle[2] + cangle[0] * cangle[2]
        return dcm
    elif rot_seq == "xyz":
        dcm[0, 0] = cangle[1] * cangle[2]
        dcm[0, 1] = sangle[0] * sangle[1] * cangle[2] + cangle[0] * sangle[2]
        dcm[0, 2] = -cangle[0] * sangle[1] * cangle[2] + sangle[0] * sangle[2]
        dcm[1, 0] = -cangle[1] * sangle[2]
        dcm[1, 1] = -sangle[0] * sangle[1] * sangle[2] + cangle[0] * cangle[2]
        dcm[1, 2] = cangle[0] * sangle[1] * sangle[2] + sangle[0] * cangle[2]
        dcm[2, 0] = sangle[1]
        dcm[2, 1] = -sangle[0] * cangle[1]
        dcm[2, 2] = cangle[0] * cangle[1]
        return dcm
    elif rot_seq == "xyx":
        dcm[0, 0] = cangle[1]
        dcm[0, 1] = sangle[0] * sangle[1]
        dcm[0, 2] = -cangle[0] * sangle[1]
        dcm[1, 0] = sangle[1] * sangle[2]
        dcm[1, 1] = -sangle[0] * cangle[1] * sangle[2] + cangle[0] * cangle[2]
        dcm[1, 2] = cangle[0] * cangle[1] * sangle[2] + sangle[0] * cangle[2]
        dcm[2, 0] = sangle[1] * cangle[2]
        dcm[2, 1] = -sangle[0] * cangle[2] * cangle[1] - cangle[0] * sangle[2]
        dcm[2, 2] = cangle[0] * cangle[2] * cangle[1] - sangle[0] * sangle[2]
        return dcm
    elif rot_seq == "xzy":
        dcm[0, 0] = cangle[2] * cangle[1]
        dcm[0, 1] = cangle[0] * cangle[2] * sangle[1] + sangle[0] * sangle[2]
        dcm[0, 2] = sangle[0] * cangle[2] * sangle[1] - cangle[0] * sangle[2]
        dcm[1, 0] = -sangle[1]
        dcm[1, 1] = cangle[0] * cangle[1]
        dcm[1, 2] = sangle[0] * cangle[1]
        dcm[2, 0] = sangle[2] * cangle[1]
        dcm[2, 1] = cangle[0] * sangle[1] * sangle[2] - sangle[0] * cangle[2]
        dcm[2, 2] = sangle[0] * sangle[1] * sangle[2] + cangle[0] * cangle[2]
        return dcm
    elif rot_seq == "xzx":
        dcm[0, 0] = cangle[1]
        dcm[0, 1] = cangle[0] * sangle[1]
        dcm[0, 2] = sangle[0] * sangle[1]
        dcm[1, 0] = -sangle[1] * cangle[2]
        dcm[1, 1] = cangle[0] * cangle[2] * cangle[1] - sangle[0] * sangle[2]
        dcm[1, 2] = sangle[0] * cangle[2] * cangle[1] + cangle[0] * sangle[2]
        dcm[2, 0] = sangle[1] * sangle[2]
        dcm[2, 1] = -cangle[0] * cangle[1] * sangle[2] - sangle[0] * cangle[2]
        dcm[2, 2] = -sangle[0] * cangle[1] * sangle[2] + cangle[0] * cangle[2]
        return dcm
    else:
        return False


def dcm2euler(dcm, rot_seq="zyx"):
    """
    Convert direction cosine matrix to Euler angles.
    The Euler angles rotate the frame n to the frame b according to specified
    rotation sequency. The DCM is a 3x3 coordinate transformation matrix from n
    to b. That is v_b  = DCM * v_n. '_b' or '_n' mean the vector 'v' is expressed
    in the frame b or n.
    Args:
        dcm: 3x3 coordinate transformation matrix from n to b
        rot_seq: rotation sequence corresponding to the angles.
    Returns:
        angles: 3x1 Euler angles, rad.
    """
    if rot_seq == "zyx":
        #     [          cy*cz,          cy*sz,            -sy]
        #     [ sy*sx*cz-sz*cx, sy*sx*sz+cz*cx,          cy*sx]
        #     [ sy*cx*cz+sz*sx, sy*cx*sz-cz*sx,          cy*cx]
        [r1, r2, r3] = three_axis_rot(dcm[0][1], dcm[0][0], -dcm[0][2], dcm[1][2], dcm[2][2])
        return np.array([r1, r2, r3])
    elif rot_seq == "zyz":
        #     [  cz2*cy*cz-sz2*sz,  cz2*cy*sz+sz2*cz,           -cz2*sy]
        #     [ -sz2*cy*cz-cz2*sz, -sz2*cy*sz+cz2*cz,            sz2*sy]
        #     [             sy*cz,             sy*sz,                cy]
        [r1, r2, r3] = two_axis_rot(dcm[2][1], dcm[2][0], dcm[2][2], dcm[1][2], -dcm[0][2])
        return np.array([r1, r2, r3])
    elif rot_seq == "zxy":
        #     [ cy*cz-sy*sx*sz, cy*sz+sy*sx*cz,         -sy*cx]
        #     [         -sz*cx,          cz*cx,             sx]
        #     [ sy*cz+cy*sx*sz, sy*sz-cy*sx*cz,          cy*cx]
        [r1, r2, r3] = three_axis_rot(-dcm[1][0], dcm[1][1], dcm[1][2], -dcm[0][2], dcm[2][2])
        return np.array([r1, r2, r3])
    elif rot_seq == "zxz":
        #     [  cz2*cz-sz2*cx*sz,  cz2*sz+sz2*cx*cz,            sz2*sx]
        #     [ -sz2*cz-cz2*cx*sz, -sz2*sz+cz2*cx*cz,            cz2*sx]
        #     [             sz*sx,            -cz*sx,                cx]
        [r1, r2, r3] = two_axis_rot(dcm[2][0], -dcm[2][1], dcm[2][2], dcm[0][2], dcm[1][2])
        return np.array([r1, r2, r3])
    elif rot_seq == "yxz":
        #     [  cy*cz+sy*sx*sz,           sz*cx, -sy*cz+cy*sx*sz]
        #     [ -cy*sz+sy*sx*cz,           cz*cx,  sy*sz+cy*sx*cz]
        #     [           sy*cx,             -sx,           cy*cx]
        [r1, r2, r3] = three_axis_rot(dcm[2][0], dcm[2][2], -dcm[2][1], dcm[0][1], dcm[1][1])
        return np.array([r1, r2, r3])
    elif rot_seq == "yxy":
        #     [  cy2*cy-sy2*cx*sy,            sy2*sx, -cy2*sy-sy2*cx*cy]
        #     [             sy*sx,                cx,             cy*sx]
        #     [  sy2*cy+cy2*cx*sy,           -cy2*sx, -sy2*sy+cy2*cx*cy]
        [r1, r2, r3] = two_axis_rot(dcm[1][0], dcm[1][2], dcm[1][1], dcm[0][1], -dcm[2][1])
        return np.array([r1, r2, r3])
    elif rot_seq == "yzx":
        #     [           cy*cz,              sz,          -sy*cz]
        #     [ -sz*cx*cy+sy*sx,           cz*cx,  sy*cx*sz+cy*sx]
        #     [  cy*sx*sz+sy*cx,          -cz*sx, -sy*sx*sz+cy*cx]
        [r1, r2, r3] = three_axis_rot(-dcm[0][2], dcm[0][0], dcm[0][1], -dcm[2][1], dcm[1][1])
        return np.array([r1, r2, r3])
    elif rot_seq == "yzy":
        #     [  cy2*cz*cy-sy2*sy,            cy2*sz, -cy2*cz*sy-sy2*cy]
        #     [            -cy*sz,                cz,             sy*sz]
        #     [  sy2*cz*cy+cy2*sy,            sy2*sz, -sy2*cz*sy+cy2*cy]
        [r1, r2, r3] = two_axis_rot(dcm[1][2], -dcm[1][0], dcm[1][1], dcm[2][1], dcm[0][1])
        return np.array([r1, r2, r3])
    elif rot_seq == "xyz":
        #     [          cy*cz, sz*cx+sy*sx*cz, sz*sx-sy*cx*cz]
        #     [         -cy*sz, cz*cx-sy*sx*sz, cz*sx+sy*cx*sz]
        #     [             sy,         -cy*sx,          cy*cx]
        [r1, r2, r3] = three_axis_rot(-dcm[2][1], dcm[2][2], dcm[2][0], -dcm[1][0], dcm[0][0])
        return np.array([r1, r2, r3])
    elif rot_seq == "xyx":
        #     [                cy,             sy*sx,            -sy*cx]
        #     [            sx2*sy,  cx2*cx-sx2*cy*sx,  cx2*sx+sx2*cy*cx]
        #     [            cx2*sy, -sx2*cx-cx2*cy*sx, -sx2*sx+cx2*cy*cx]
        [r1, r2, r3] = two_axis_rot(dcm[0][1], -dcm[0][2], dcm[0][0], dcm[1][0], dcm[2][0])
        return np.array([r1, r2, r3])
    elif rot_seq == "xzy":
        #     [          cy*cz, sz*cx*cy+sy*sx, cy*sx*sz-sy*cx]
        #     [            -sz,          cz*cx,          cz*sx]
        #     [          sy*cz, sy*cx*sz-cy*sx, sy*sx*sz+cy*cx]
        [r1, r2, r3] = three_axis_rot(dcm[1][2], dcm[1][1], -dcm[1][0], dcm[2][0], dcm[0][0])
        return np.array([r1, r2, r3])
    elif rot_seq == "xzx":
        #     [                cz,             sz*cx,             sz*sx]
        #     [           -cx2*sz,  cx2*cz*cx-sx2*sx,  cx2*cz*sx+sx2*cx]
        #     [            sx2*sz, -sx2*cz*cx-cx2*sx, -sx2*cz*sx+cx2*cx]
        [r1, r2, r3] = two_axis_rot(dcm[0][2], dcm[0][1], dcm[0][0], dcm[2][0], -dcm[1][0])
        return np.array([r1, r2, r3])
    else:
        return False


def ecef_to_ned(lat, lon):
    """
    transformation matrix from the ECEF frame to the NED frame defined by lat and lon.
    Args:
        lat: latitude, rad
        lon: longitude, rad
    """
    return rot_y(-math.pi / 2.0 - lat).dot(rot_z(lon))


def three_axis_rot(r11, r12, r21, r31, r32):
    r1 = math.atan2(r11, r12)
    r2 = math.asin(r21)
    r3 = math.atan2(r31, r32)
    return r1, r2, r3


def two_axis_rot(r11, r12, r21, r31, r32):
    r1 = math.atan2(r11, r12)
    r2 = math.acos(r21)
    r3 = math.atan2(r31, r32)
    return np.array([r1, r2, r3])


def rot_x(angle):
    """
    Coordinate transformation matrix from the original frame to the frame after
    rotation when rotating about x axis
    Args:
        angle: rotation angle, rad
    Returns:
        rx: 3x3 orthogonal matrix
    """
    sangle = math.sin(angle)
    cangle = math.cos(angle)
    rx = np.array([[1.0, 0.0, 0.0], [0.0, cangle, sangle], [0.0, -sangle, cangle]])
    return rx


def rot_y(angle):
    """
    Coordinate transformation matrix from the original frame to the frame after
    rotation when rotating about y axis
    Args:
        angle: rotation angle, rad
    Returns:
        ry: 3x3 orthogonal matrix
    """
    sangle = math.sin(angle)
    cangle = math.cos(angle)
    ry = np.array([[cangle, 0.0, -sangle], [0.0, 1.0, 0.0], [sangle, 0.0, cangle]])
    return ry


def rot_z(angle):
    """
    Coordinate transformation matrix from the original frame to the frame after
    rotation when rotating about z axis
    Args:
        angle: rotation angle, rad
    Returns:
        rz: 3x3 orthogonal matrix
    """
    sangle = math.sin(angle)
    cangle = math.cos(angle)
    rz = np.array([[cangle, sangle, 0.0], [-sangle, cangle, 0.0], [0.0, 0.0, 1.0]])
    return rz


def quat_update(q, w, dt):
    """
    Args:
        q: quaternion, scalar first.
        w: angular velocity, rad/s.
        dt: sample period, sec.
    Returns:
        q: updated quaternion.
    """
    rot_quat = rotation_quat(w, dt)
    q = quat_multiply(q, rot_quat)
    q = quat_normalize(q)
    return q


def euler_update_zyx(x, w, dt):
    """
    Update Euler angles.
    Rotation sequence is zyx [phi theta psi], rad.
    Args:
        x: input Euler angles, rad
        w: angular velocity, rad/s
        dt: time interval, sec
    Returns:
        y: propagate x according to angular velocity w for dt seconds
    """
    # Euler angle rate
    c_psi = math.cos(x[2])
    s_psi = math.sin(x[2])
    phi_dot = (w[2] * c_psi + w[1] * s_psi) / math.cos(x[1])
    theta_dot = w[1] * c_psi - w[2] * s_psi
    psi_dot = w[0] + (w[2] * c_psi + w[1] * s_psi) * math.tan(x[1])

    y = np.copy(x)  # y = x will cause x modified when modifying y
    y[0] += phi_dot * dt
    y[1] += theta_dot * dt
    y[2] += psi_dot * dt
    # limit value
    # pitch is more complicated
    if y[1] > HALF_PI:
        y[1] = math.pi - y[1]
        y[0] += math.pi
        y[2] += math.pi
    elif y[1] < -HALF_PI:
        y[1] = -math.pi - y[1]
        y[0] += math.pi
        y[2] += math.pi
    # yaw is within [-pi, pi]
    if y[0] > math.pi:
        y[0] -= TWO_PI
    elif y[0] < -math.pi:
        y[0] += TWO_PI
    # roll is within [-pi, pi]
    if y[2] > math.pi:
        y[2] -= TWO_PI
    elif y[2] < -math.pi:
        y[2] += TWO_PI
    return y


def rotation_quat(w, dt):
    """
    Args:
        w: angular velocity, rad/s.
        dt: sample period, sec.
    Returns:
        rot_quat: rotation quaternion corresponds to w and dt
    """
    rot_vec = w * dt  # rotation vector
    theta = math.sqrt(np.dot(rot_vec, rot_vec))  # rotation angle
    half_theta = 0.5 * theta  # half rotation angle
    s = math.sin(half_theta)
    c = math.cos(half_theta)
    if theta == 0.0:
        return np.array([1.0, 0.0, 0.0, 0.0])
    elif c >= 0:
        tmp = s / theta
        return np.array([c, tmp * rot_vec[0], tmp * rot_vec[1], tmp * rot_vec[2]])
    else:
        tmp = -s / theta
        return np.array([-c, tmp * rot_vec[0], tmp * rot_vec[1], tmp * rot_vec[2]])


def get_cross_mtx(a):
    """
    x = cross(a, b) = a_cross * b. This function generate a_cross from a.
    Args:
        a: (3,) array.
    Returns:
        a_cross: (3,3) matrix
    """
    a_cross = np.array([[0.0, -a[2], a[1]], [a[2], 0.0, -a[0]], [-a[1], a[0], 0.0]])
    return a_cross


def cross3(a, b):
    """
    cross product of array of size 3.
    Args:
        a: array of size 3.
        b: array of size 3.
    Returns:
        c: c = cross(a,b), of size 3.
    """
    c = np.array([a[1] * b[2] - a[2] * b[1], a[2] * b[0] - a[0] * b[2], a[0] * b[1] - a[1] * b[0], ])
    return c


def euler_angle_range_three_axis(angles):
    """
    Limit Euler angle range.
    For three-axis rotation, the angle ranges are [-pi, pi], [-pi/2, pi/2] and [-pi, pi]
    For two-axis rotation, the angle ranges are [-pi, pi], [0, pi] and [-pi, pi]
    Args:
        angles: numpy array of (3,) or (3,1)

    """
    half_pi = 0.5 * math.pi
    # convert the second angle in range [-pi, pi]
    a1 = angles[0]
    a2 = angle_range_pi(angles[1])
    a3 = angles[2]
    # the second angle is not within [-pi/2, pi/2]?
    if a2 > half_pi:
        a2 = math.pi - a2
        a1 += math.pi
        a3 += math.pi
    elif a2 < -half_pi:
        a2 = -math.pi - a2
        a1 += math.pi
        a3 += math.pi
    a1 = angle_range_pi(a1)
    a3 = angle_range_pi(a3)
    return np.array([a1, a2, a3])


def angle_range_pi(x):
    """
    Limit angle range within [-pi, pi]
    Args：
        x: rad
    Return:
        equivalent angle of x, [-pi, pi], rad
    """
    # [0, 2pi]
    x %= TWO_PI
    # [-pi, pi]
    if x > math.pi:
        x -= TWO_PI
    return x


class Sim_data(object):
    """
    Simulation data
    """

    def __init__(self, name, description, units=None, output_units=None, plottable=True, logx=False, logy=False,
            grid="on", legend=None, ):
        """
        Set up data properties and plot properties. All data are stored in a dict (or a scalr or
        a numpy array): self.data.
        Each key of this dict corresponds to a set of data. self.data[key] is of size mxn.
        m is the number of samples of this set of data. n is the dimension of this set of data.
        m may vary through different set of data. n must be same for all sets of data.
        Args:
            name: string name of the data
            description: string description of the data
            units: a tuple or list of strings to specify units of data.
                The length of units is the same as columns of each set of data in self.data.
            output_units: a tuple or list of strings to specify units of data when we plot or
                save the data to files. Sim_data.plot and Sim_data.save_to_file will automatically
                convert units if necessary.
                If this is set to None, output_units will be the same as units, and no unit
                conversion is needed.
            logx: plot this data with log scaling on x axis
            logy: plot this data with log scaling on y axis
            grid: if this is not 'off', it will be changed to 'on'
            legend: tuple or list of strings to specify legend of data.
                The length of units is the same as columns of each set of data in self.data.
        """
        self.name = name
        self.description = description
        # units of self.data
        if units is None:
            self.units = [""]
        else:
            self.units = list(units)
        # output units should have same length as units
        if output_units is None:
            self.output_units = self.units
        else:
            self.output_units = list(output_units)
            len_in = len(self.units)
            len_out = len(self.output_units)
            if len_in > len_out:
                for i in range(len_out, len_in):
                    self.output_units.append(self.units[i])
            elif len_in < len_out:
                for i in range(len_in, len_out):
                    self.units.append(self.output_units[i])
        self.plottable = plottable
        self.logx = logx
        self.logy = logy
        self.grid = "on"
        if grid.lower() == "off":
            self.grid = grid
        self.legend = legend
        """
        each item in the data should be either scalar or numpy.array of size(n, dim),
        or a dict of the above two, dict keys are like 0, 1, 2, 3, ...
        n is the sample number, dim is a set of data at time tn. For example, accel is nx3,
        att_quat is nx4, allan_t is (n,)
        """
        self.data = {}

    def add_data(self, data, key=None, units=None):
        """
        Add data to Sim_data.
        Args:
            data: a scalar, a numpy array or a dict of the above two. If data is a dict, each
                value in it should be of same type (scalr or numpy array), same size and same
                units.
            key: There are more than one set of data, key is an index of data added this time.
                If key is None, data can be a scalr, a numpy array or a dict of the above two.
                If key is a valid dict key, data can be a scalar or a numpy.
            units: Units of the input data. If you know clearly no units convertion is needed, set
                units to None. If you do not know what units are used in the class InsDataMgr,
                you'd better provide the units of the data. Units convertion will be done
                automatically here.
                If data is a scalar, units should be a list of one string to define its unit.
                If data is a numpy of size(m,n), units should be a list of n strings
                to define the units.
                If data is a dict, units should be the same as the above two depending on if
                each value in the dict is a scalr or a numpy array.
        """
        # units convertion should be done in sim_data.py
        if units is not None:
            units = list(units)  # units of Sim_data is a list even if it contains scalars
            if len(units) == len(self.units):
                if units != self.units:
                    # data units are different from units in the manager, need convertion
                    data = convert_unit(data, units, self.units)
            else:
                logging.debug(units)
                logging.debug(self.units)
                raise ValueError("Units are of different lengths.")
        # add data into the manager
        if key is None:
            self.data = data
        else:
            if not isinstance(self.data, dict):
                self.data = {}
            self.data[key] = data

    def save_to_file(self, data_dir):
        """
        Save self.data to files.
        Args:
            data_dir: directory for the data files.
        """
        #### generate header
        # how many columns in each set of data? 0 if scalar
        cols = 0
        if isinstance(self.data, dict):
            for i in self.data:
                if self.data[i].ndim > 1:
                    cols = self.data[i].shape[1]
                break  # each set of data in data should have the same number of columns
        elif isinstance(self.data, np.ndarray):
            if self.data.ndim > 1:
                cols = self.data.shape[1]
        # add the name and unit of each column to header
        header_line = ""
        if cols > 0:  # more than one column
            for i in range(cols):
                # units
                str_unit = ""
                if i < len(self.output_units):
                    str_unit = " (" + self.output_units[i] + ")"
                # add a column
                if (self.legend is not None) and (cols == len(self.legend)):  # legend available
                    header_line += self.legend[i] + str_unit + ","
                else:  # legend not available
                    header_line += self.name + "_" + str(i) + str_unit + ","
            # remove the trailing ','
            header_line = header_line[0:-1]
        else:  # only one column
            str_unit = ""
            if len(self.output_units) > 0:
                str_unit = " (" + self.output_units[0] + ")"
            header_line = self.name + str_unit
        #### save data and header to .csv files
        if isinstance(self.data, dict):
            for i in self.data:
                file_name = data_dir + "//" + self.name + "-" + str(i) + ".csv"
                np.savetxt(file_name, convert_unit(self.data[i], self.units, self.output_units), header=header_line,
                        delimiter=",", comments="", )
        else:
            file_name = data_dir + "//" + self.name + ".csv"
            np.savetxt(file_name, convert_unit(self.data, self.units, self.output_units), header=header_line,
                    delimiter=",", comments="", )


def convert_unit(data, src_unit, dst_unit):
    """
    Unit conversion. Notice not to change values in data
    Args:
        data: convert data units from src_unit to dst_unit. Data should be a scalar,
            a numpy array of size(n,) or (n,m). n is data length, m is data dimension.
        src_unit: a list of unit of the data.
        dst_unit: a list of unit we want to convert the data to.
    Returns:
        x: data after unit conversion.
    """
    scale = unit_conversion_scale(src_unit, dst_unit)
    # unit conversion
    x = data.copy()  # avoid changing values in data
    if isinstance(x, dict):
        for i in x:
            x[i] = convert_unit_ndarray_scalar(x[i], scale)
    else:
        x = convert_unit_ndarray_scalar(x, scale)
    return x


def unit_conversion_scale(src_unit, dst_unit):
    """
    Calculate unit conversion scale.
    """
    m = len(dst_unit)
    scale = np.ones((m,))
    for i in range(m):
        # deg to rad
        if src_unit[i] == "deg" and dst_unit[i] == "rad":
            scale[i] = D2R
        elif src_unit[i] == "deg/s" and dst_unit[i] == "rad/s":
            scale[i] = D2R
        elif src_unit[i] == "deg/hr" and dst_unit[i] == "rad/s":
            scale[i] = D2R / 3600.0
        # rad to deg
        elif src_unit[i] == "rad" and dst_unit[i] == "deg":
            scale[i] = 1.0 / D2R
        elif src_unit[i] == "rad/s" and dst_unit[i] == "deg/s":
            scale[i] = 1.0 / D2R
        elif src_unit[i] == "rad/s" and dst_unit[i] == "deg/hr":
            scale[i] = 3600.0 / D2R
        else:
            if src_unit[i] != dst_unit[i]:
                logging.debug("Cannot convert unit from %s in %s to %s." % (src_unit[i], src_unit, dst_unit[i]))
    return scale


def convert_unit_ndarray_scalar(x, scale):
    """
    Unit conversion of numpy array or a scalar.
    Args:
        x: convert x units from src_unit to dst_unit. x should be a scalar,
            a numpy array of size(n,) or (n,m). n is x length, m is x dimension.
        scale: 1D numpy array of unit convertion scale. x = x * scale
    Returns:
        x: x after unit conversion.
    """
    m = scale.shape[0]
    if isinstance(x, np.ndarray):
        if x.ndim == 2:
            for i in range(min(m, x.shape[1])):
                if scale[i] != 1.0:
                    x[:, i] = x[:, i] * scale[i]
        elif x.ndim == 1:
            if len(x) == m:
                x *= scale
            else:
                x = x * scale[0]
    elif isinstance(x, (int, float)):
        x = x * scale[0]
    else:
        raise ValueError("Input x should be a scalar, 1D or 2D array, ndim = %s" % x.ndim)
    return x


def path_gen(ini_pos_vel_att, motion_def, output_def, mobility, ref_frame=0, magnet=False):
    """
    Generate IMU and GPS or odometer data file according to initial position\velocity\attitude,
    motion command and simulation mode.
    The navigation frame is NED. The body frame is front-right-downward. True IMU sensor
    data, position\velocity\attitude data and GPS or odometry measurement data are
    generated and stored in files.
    Units for position in LLA [Lat Lon Alt] form are [rad rad m], units for position in [x y z]
    form is [m m m], units for angles are rad, time unit is sec, unless specified otherwise.
    Args:
        ini_pos_vel_att: 9x1 initial position, velocity and attitude.
            3x1 position in the form of [Lat, Lon, Alt].
            3x1 velocity in the body frame.
            3x1 attitude in Euler angles [yaw, pitch, roll], rotation sequency is zyx.
        motion_def: nx6 motion definitions. Each row defines command of a motion segment.
            motion_def[:,0]: motion type.
                1: motion params directly give body frame velocity and Euler angles change rate.
                2: absolute att and absolute vel to rech.
                3: relative att and vel change.
                4: absolute att, relative vel.
                5: relative att, absolute vel.
            motion_def[:,1:6]: motion params = [Att command, vel along body axis command].
            motion_def[:,7] = maximum time for the given segment, sec.
        output_def: [[simulation_over_sample_rate imu_freq];
                     [1 gps_freq]
                     [1 odo_freq]], Hz.
                     1 means to enable GPS or odometer, otherwise to disable.
        mobility: [max_acceleration, max_angular_acceleration, max_angular_velocity]
        ref_frame: reference frame used as the navigation frame,
            0: NED (default).
            1: a virtual inertial frame, with constant g and z axis pointing along g.
        magnet:
            False: Geomagnetic field in the body frame will not be calculaed.
            True: Geomagnetic field in the body frame will be calculaed.
                For ref_frame==0, N is geographic north, and there is declination;
                For ref_frame==1, there is no declination.
    Returns:
        path_results. Resutls of path generation.
            'status':  True: Everything is OK.
                        False:
            'imu':      True/ideal IMU measurements. Each line is organized as [index, acc, gyro],
                        index is an interger and increases at imu_output_freq.
            'nav':      True position, velocity and attitude (Euler angles, ZYX).
                        ref_frame==0, [index, absolute_position_lla, velocity_in_NED_frame, attitude],
                        ref_frame==1, [index, absolute_position_xyz, velocity_in_NED_frame, attitude],
                        Notice: For absolute_position_xyz, it is indeed the sum of the initial
                        position in ecef and the relative position in the virutal inertial frame.
                        Index is synced with index in mimu.csv.
            'mag':      True/ideal geomagneti field in the body frame.
                        [index, magx, magy, magz], uT, index synced with mimu.csv index.
            'gps':      True GPS measurements.
                        ref_frame==0, [index, absolute_position_lla, velocity_in_navigation_frame],
                        ref_frame==1, [index, absolute_position_xyz, velocity_in_navigation_frame],
                        GPS data are down sampled to gps_freq, index synced with mimu.csv index.
                        Note that the navigation frame is not the ECEF frame. That is, if ref_frame
                        is 0, velocity_in_navigation_frame is NED velocity.
            'odo':      True odometer measurements.
                        [index, travel_distance, velocity_in_body_frame].
                        Odometry are down sampled to odd_freq, index synced with mimu.csv index.
    """
    ### path generation results
    path_results = {
            "status": True, "imu": [], "nav": [], "mag": [], "gps": [], "odo": [],
    }

    ### sim freq and data output freq
    out_freq = output_def[0, 1]  # IMU output frequency
    sim_osr = output_def[0, 0]  # simulation over sample ratio w.r.t IMU output freq
    sim_freq = sim_osr * out_freq  # simulation frequency
    dt = 1.0 / sim_freq  # simulation period

    ### Path gen command filter to make trajectory smoother
    alpha = 0.9  # for the low pass filter of the motion commands
    filt_a = alpha * np.eye(3)
    filt_b = (1 - alpha) * np.eye(3)
    max_acc = mobility[0]  # 10.0m/s2, max acceleratoin
    max_dw = mobility[1]  # 0.5rad/s2    # max angular acceleration, rad/s/s
    max_w = mobility[2]  # 1.0rad/s       # max angular velocity, rad/s
    kp = 5.0  # kp and kd are PD controller params
    kd = 10.0
    att_converge_threshold = 1e-4  # threshold to determine if the command is completed
    vel_converge_threshold = 1e-4
    att_dot = np.zeros(3)  # Euler angle change rate
    vel_dot_b = np.zeros(3)  # Velocity change rate in the body frame

    ### convert time duration to simulation cycles
    sim_count_max = 0
    for i in range(0, motion_def.shape[0]):
        if motion_def[i, 7] < 0:
            raise ValueError("Time duration of %s-th command has negative time duration: %s." % (i, motion_def[i, 7]))
        seg_count = motion_def[i, 7] * out_freq  # max count for this segment
        sim_count_max += math.ceil(seg_count)  # data count of all segments
        motion_def[i, 7] = round(seg_count * sim_osr)  # simulation count
    # total sim_count_max must be above 0
    if sim_count_max <= 0:
        raise ValueError("Total time duration in the motion definition file must be above 0.")
    ### create output arrays
    sim_count_max = int(sim_count_max)
    imu_data = np.zeros((sim_count_max, 7))
    nav_data = np.zeros((sim_count_max, 10))
    enable_gps = False
    enable_odo = False
    if output_def.shape[0] == 3:
        if output_def[1, 0] == 1:
            enable_gps = True
            gps_data = np.zeros((sim_count_max, 8))
            output_def[1, 1] = sim_osr * round(out_freq / output_def[1, 1])
        else:
            output_def[1, 0] = -1
        if output_def[2, 0] == 1:
            enable_odo = True
            odo_data = np.zeros((sim_count_max, 5))
            output_def[2, 1] = sim_osr * round(out_freq / output_def[2, 1])
        else:
            output_def[2, 0] = -1
    else:
        raise ValueError("output_def should be of size 3x2.")
    if magnet:
        mag_data = np.zeros((sim_count_max, 4))

    ### start computations
    sim_count = 0  # number of total simulation data
    acc_sum = np.zeros(3)  # accum of over sampled simulated acc data
    gyro_sum = np.zeros(3)  # accum of over sampled simulated gyro data
    odo_dist = 0  # accum of travel distance
    ## initialize
    pos_n = ini_pos_vel_att[0:3]  # ini pos, LLA
    vel_b = ini_pos_vel_att[3:6]  # ini vel
    att = ini_pos_vel_att[6:9]  # ini att
    c_nb = euler2dcm(att).T  # b to n
    vel_n = c_nb.dot(vel_b)
    pos_delta_n = np.zeros(3)  # pos change
    earth_param = geo_param(pos_n)  # geo parameters
    g = earth_param[2]  # local gravity at ini pos
    if magnet:  # geomagnetic parameters at the initial position
        gm = GeoMag()
        geo_mag = gm.GeoMag(pos_n[0] / D2R, pos_n[1] / D2R, pos_n[2])  # units in nT and deg
        geo_mag_n = np.array([geo_mag.bx, geo_mag.by, geo_mag.bz])
        geo_mag_n /= 1000.0  # nT to uT
        if ref_frame == 1:  # remove inclination
            geo_mag_n[0] = math.sqrt(geo_mag_n[0] * geo_mag_n[0] + geo_mag_n[1] * geo_mag_n[1])
            geo_mag_n[1] = 0.0
    ## start trajectory generation
    if ref_frame == 1:  # if using virtual inertial frame, convert LLA to ECEF xyz
        pos_n = lla2ecef(pos_n)
    idx_high_freq = 0  # data index for imu, nav, mag
    idx_low_freq = 0  # data index for gps, odo
    for i in range(0, motion_def.shape[0]):
        com_type = round(motion_def[i, 0])  # command type of this segment
        gps_visibility = motion_def[i, 8]  # gps visibility
        # get command of this segment
        """if i == 2:
            logging.debug("haha")"""
        motion_com = parse_motion_def(motion_def[i], att, vel_b)
        if com_type == 1:
            att_dot_com = motion_com[0]
            vel_dot_com = motion_com[1]
        else:
            att_com = motion_com[0]
            vel_com_b = motion_com[1]
        # initialize the filter states to last att and vel
        att_com_filt = att
        vel_com_b_filt = vel_b
        # generate trajectory according to the command of this segment
        sim_count_max = (sim_count + motion_def[i, 7])  # max cycles to execute command of this seg
        com_complete = 0  # complete command of this seg, go to next
        while (sim_count < sim_count_max) and (com_complete == 0):
            # handle the input motion commands
            if com_type == 1:
                att_dot = filt_a.dot(att_dot) + filt_b.dot(att_dot_com)  # filter input
                vel_dot_b = filt_a.dot(vel_dot_b) + filt_b.dot(vel_dot_com)  # vel_dot_b = np.array(vel_dot_com)
            else:
                att_com_filt = filt_a.dot(att_com_filt) + filt_b.dot(att_com)  # filter command
                vel_com_b_filt = filt_a.dot(vel_com_b_filt) + filt_b.dot(vel_com_b)
                # Close the loop. Velocity change is acceleration and sudden change in acceleration
                # is reasonable. Attitude change is angular velocity and sudden change in angular
                # velocity means inifite torque, which is unreasonable. So a simple PD controller
                # is used here to track the commanded attitude.
                # acc
                vel_dot_b = (vel_com_b_filt - vel_b) / dt
                vel_dot_b[vel_dot_b > max_acc] = max_acc  # limit acceleration
                vel_dot_b[vel_dot_b < -max_acc] = -max_acc
                # w
                att_dot_dot = kp * (att_com - att) + kd * (0 - att_dot)  # feedback control
                att_dot_dot[att_dot_dot > max_dw] = max_dw  # limit w change rate
                att_dot_dot[att_dot_dot < -max_dw] = -max_dw
                att_dot = att_dot + att_dot_dot * dt
                att_dot[att_dot > max_w] = max_w  # limit att change rate
                att_dot[att_dot < -max_w] = -max_w
                # Complete the command of this segment?
                if (np.sqrt(np.dot(att - att_com, att - att_com)) < att_converge_threshold and np.sqrt(
                        np.dot(vel_b - vel_com_b, vel_b - vel_com_b)) < vel_converge_threshold):
                    com_complete = 1  # att_dot = (att_com - att) / dt  # vel_dot_b = (vel_com_b - vel_b) / dt

            # compute IMU outputs according to pos/vel/att changes
            imu_results = calc_true_sensor_output(pos_n + pos_delta_n, vel_b, att, c_nb, vel_dot_b, att_dot, ref_frame,
                    g)
            acc = imu_results[0]
            gyro = imu_results[1]
            pos_dot_n = imu_results[3]  # lla change rate if NED, vel_n if virtual inertial
            # update IMU results
            acc_sum = acc_sum + acc
            gyro_sum = gyro_sum + gyro
            # update GPS results

            # update odometer results
            odo_vel = vel_b

            # Write the results. Simulation data are down sampled according to freq specified
            # in output_def.
            # IMU measurement and navigation results
            if (sim_count % sim_osr) == 0:
                # average over sampled IMU data
                acc_avg = acc_sum / sim_osr
                gyro_avg = gyro_sum / sim_osr
                # write to files
                # imu_data[idx_high_freq, :] = np.hstack((idx_high_freq, acc_avg, gyro_avg))
                imu_data[idx_high_freq, 0] = sim_count
                imu_data[idx_high_freq, 1] = acc_avg[0]
                imu_data[idx_high_freq, 2] = acc_avg[1]
                imu_data[idx_high_freq, 3] = acc_avg[2]
                imu_data[idx_high_freq, 4] = gyro_avg[0]
                imu_data[idx_high_freq, 5] = gyro_avg[1]
                imu_data[idx_high_freq, 6] = gyro_avg[2]
                # nav data
                nav_data[idx_high_freq, 0] = sim_count
                nav_data[idx_high_freq, 1] = pos_n[0] + pos_delta_n[0]
                nav_data[idx_high_freq, 2] = pos_n[1] + pos_delta_n[1]
                nav_data[idx_high_freq, 3] = pos_n[2] + pos_delta_n[2]
                nav_data[idx_high_freq, 4] = vel_n[0]
                nav_data[idx_high_freq, 5] = vel_n[1]
                nav_data[idx_high_freq, 6] = vel_n[2]
                euler_angles = euler_angle_range_three_axis(att)
                nav_data[idx_high_freq, 7] = euler_angles[0]  # yaw [-pi, pi]
                nav_data[idx_high_freq, 8] = euler_angles[1]  # pitch [-pi/2, pi/2]
                nav_data[idx_high_freq, 9] = euler_angles[2]  # roll [-pi, pi]
                # next cycle
                acc_sum = np.zeros(3)
                gyro_sum = np.zeros(3)
                # update magnetometer results
                if magnet:
                    geo_mag_b = c_nb.T.dot(geo_mag_n)
                    # mag_data[idx_high_freq, :] = np.hstack((idx_high_freq, geo_mag_b))
                    mag_data[idx_high_freq, 0] = sim_count
                    mag_data[idx_high_freq, 1] = geo_mag_b[0]
                    mag_data[idx_high_freq, 2] = geo_mag_b[1]
                    mag_data[idx_high_freq, 3] = geo_mag_b[2]
                # update odometer results
                if enable_odo:
                    # odo_data[idx_high_freq, :] = np.hstack((idx_high_freq,
                    #                                       odo_dist, odo_vel))
                    odo_data[idx_high_freq, 0] = sim_count
                    odo_data[idx_high_freq, 1] = odo_dist
                    odo_data[idx_high_freq, 2] = odo_vel[0]
                    odo_data[idx_high_freq, 3] = odo_vel[1]
                    odo_data[idx_high_freq, 4] = odo_vel[2]
                # index increment
                idx_high_freq += 1
            # GPS or odometer measurement
            if enable_gps:
                if (sim_count % output_def[1, 1]) == 0:  # measurement period
                    gps_data[idx_low_freq, 0] = sim_count
                    gps_data[idx_low_freq, 1] = pos_n[0] + pos_delta_n[0]
                    gps_data[idx_low_freq, 2] = pos_n[1] + pos_delta_n[1]
                    gps_data[idx_low_freq, 3] = pos_n[2] + pos_delta_n[2]
                    gps_data[idx_low_freq, 4] = vel_n[0]
                    gps_data[idx_low_freq, 5] = vel_n[1]
                    gps_data[idx_low_freq, 6] = vel_n[2]
                    gps_data[idx_low_freq, 7] = gps_visibility
                    # index increment
                    idx_low_freq += 1

            # accumulate pos/vel/att change
            pos_delta_n = pos_delta_n + pos_dot_n * dt  # accumulated pos change
            odo_dist = odo_dist + np.sqrt(np.dot(vel_b, vel_b)) * dt
            vel_b = vel_b + vel_dot_b * dt
            att = att + att_dot * dt
            c_nb = euler2dcm(att).T  # b to n
            vel_n = c_nb.dot(vel_b)

            # update simulation counter
            sim_count += 1

        # if command is completed, att_dot and vel_dot should be set to zero
        if com_complete == 1:
            att_dot = np.zeros(3)
            vel_dot_b = np.zeros(3)
    # return generated data
    path_results["imu"] = imu_data[0:idx_high_freq, :]
    path_results["nav"] = nav_data[0:idx_high_freq, :]
    if magnet:
        path_results["mag"] = mag_data[0:idx_high_freq, :]
    if enable_odo:
        path_results["odo"] = odo_data[0:idx_high_freq, :]
    if enable_gps:
        path_results["gps"] = gps_data[0:idx_low_freq, :]
    return path_results


def calc_true_sensor_output(pos_n, vel_b, att, c_nb, vel_dot_b, att_dot, ref_frame, g):
    """
    Calculate true IMU results from attitude change rate and velocity
    change rate.
    attitude change rate is input in the form of Euler angle derivatives and
    converted into angular velocity. Velocity change rate is expressed in
    the body frame. Position change rate is also calculated. If simulation is
    done in the NED frame, the position change rate is in the form of Lat, Lon
    and alt derivatives. Otherwise, it is given in m/s.
    Args:
        pos_n: For NED, it is the absolute LLA position. Otherwise, it is relative
            motion.
        vel_b: Velocity in the body frame, m/s.
        att: Euler angles, [yaw pitch roll], rot seq is ZYX, rad.
        c_nb: Transformation matrix from b to n corresponding to att.
        vel_dot_b: Velocity change rate in the body frame, m/s/s
        att_dot: Euler angle change rate, [yaw_d, pitch_d, roll_d], rad/s
        ref_frame: See doc of function PathGen.
        g: Gravity, only used when ref_frame==1, m/s/s.
    Returns:
        [0]: 3x1 true accelerometer output in the body frame, m/s/s
        [1]: 3x1 true gyro output in the body frame, rad/s
        [2]: 3x1 velocity change rate in the navigation frame, m/s/s
        [3]: 3x1 position change rate in the navigation frame, m/s
    """
    # velocity in N
    vel_n = c_nb.dot(vel_b)

    # Calculate rotation rate of n w.r.t e in n and e w.r.t i in n
    # For the NED frame, the NED frame rotation and Earth rotation rate is calculated
    # For the virtual inertial frame, they are not needed and simply set to zeros.
    w_en_n = np.zeros(3)
    w_ie_n = np.zeros(3)
    if ref_frame == 0:
        earth_param = geo_param(pos_n)
        rm = earth_param[0]
        rn = earth_param[1]
        g = earth_param[2]
        sl = earth_param[3]
        cl = earth_param[4]
        w_ie = earth_param[5]
        rm_effective = rm + pos_n[2]
        rn_effective = rn + pos_n[2]
        gravity = np.array([0, 0, g])
        w_en_n[0] = vel_n[1] / rn_effective  # wN
        w_en_n[1] = -vel_n[0] / rm_effective  # wE
        w_en_n[2] = -vel_n[1] * sl / cl / rn_effective  # wD
        w_ie_n[0] = w_ie * cl
        w_ie_n[2] = -w_ie * sl
    else:
        gravity = [0, 0, g]

    # Calculate rotation rate of b w.r.t n expressed in n.
    # Calculate rotation rate from Euler angle derivative using ZYX rot seq.
    sh = math.sin(att[0])
    ch = math.cos(att[0])
    w_nb_n = np.zeros(3)
    w_nb_n[0] = -sh * att_dot[1] + c_nb[0, 0] * att_dot[2]
    w_nb_n[1] = ch * att_dot[1] + c_nb[1, 0] * att_dot[2]
    w_nb_n[2] = att_dot[0] + c_nb[2, 0] * att_dot[2]
    # Calculate rotation rate from rotation quaternion
    # w_nb_n = np.zeros(3)

    # Velocity derivative
    vel_dot_n = c_nb.dot(vel_dot_b) + cross3(w_nb_n, vel_n)
    # Position derivative
    pos_dot_n = np.zeros(3)
    if ref_frame == 0:
        pos_dot_n[0] = vel_n[0] / rm_effective  # Lat
        pos_dot_n[1] = vel_n[1] / rn_effective / cl  # Lon
        pos_dot_n[2] = -vel_n[2]  # Alt
    else:
        pos_dot_n[0] = vel_n[0]
        pos_dot_n[1] = vel_n[1]
        pos_dot_n[2] = vel_n[2]
    # Gyroscope output
    gyro = c_nb.T.dot(w_nb_n + w_en_n + w_ie_n)
    # Acceleration output
    w_ie_b = c_nb.T.dot(w_ie_n)
    acc = vel_dot_b + cross3(w_ie_b + gyro, vel_b) - c_nb.T.dot(gravity)
    return acc, gyro, vel_dot_n, pos_dot_n


def parse_motion_def(motion_def_seg, att, vel):
    """
    Parse the command of a segment in motion_def.
    Args:
        motion_def_seg: a segment in motion_def
        att: current attitude
        vel: current velocity
    Returns:
        [0]: Target attitude
        [1]: Target velocity
    """
    if motion_def_seg[0] == 1:
        att_com = [motion_def_seg[1], motion_def_seg[2], motion_def_seg[3]]
        vel_com = [motion_def_seg[4], motion_def_seg[5], motion_def_seg[6]]
    elif motion_def_seg[0] == 2:  # abs att and abs vel
        att_com = [motion_def_seg[1], motion_def_seg[2], motion_def_seg[3]]
        vel_com = [motion_def_seg[4], motion_def_seg[5], motion_def_seg[6]]
    elif motion_def_seg[0] == 3:  # rel att and rel vel
        att_com = att + [motion_def_seg[1], motion_def_seg[2], motion_def_seg[3]]
        vel_com = vel + [motion_def_seg[4], motion_def_seg[5], motion_def_seg[6]]
    elif motion_def_seg[0] == 4:  # abs att and rel vel
        att_com = [motion_def_seg[1], motion_def_seg[2], motion_def_seg[3]]
        vel_com = vel + [motion_def_seg[4], motion_def_seg[5], motion_def_seg[6]]
    elif motion_def_seg[0] == 5:  # rel att and abs vel
        att_com = att + [motion_def_seg[1], motion_def_seg[2], motion_def_seg[3]]
        vel_com = [motion_def_seg[4], motion_def_seg[5], motion_def_seg[6]]
    return att_com, vel_com


def acc_gen(fs, ref_a, acc_err, vib_def=None):
    """
    Add error to true acc data according to acclerometer model parameters
    Args:
        fs: sample frequency, Hz.
        ref_a: nx3 true acc data, m/s2.
        acc_err: accelerometer error parameters.
            'b': 3x1 acc constant bias, m/s2.
            'b_drift': 3x1 acc bias drift, m/s2.
            'vrw': 3x1 velocity random walk, m/s2/root-Hz.
        vib_def: Vibration model and parameters. Vibration type can be random, sinunoida or
            specified by single-sided PSD.
            Generated vibrating acc is expressed in the body frame.
            'type' == 'random':
                Normal distribution. 'x', 'y' and 'z' give the 1sigma values along x, y and z axis.
                units: m/s2
            'type' == 'sinunoidal'
                Sinunoidal vibration. 'x', 'y' and 'z' give the amplitude of the sine wave along
                x, y and z axis. units: m/s2.
            'type' == 'psd'. Single sided PSD.
                'freq':  frequency, in unit of Hz
                'x': x axis, in unit of m2/s4/Hz.
                'y': y axis, in unit of m2/s4/Hz.
                'z': z axis, in unit of m2/s4/Hz.
    Returns:
        a_mea: nx3 measured acc data
    """
    dt = 1.0 / fs
    # total data count
    n = ref_a.shape[0]
    ## simulate sensor error
    # static bias
    acc_bias = acc_err["b"]
    # bias drift
    acc_bias_drift = bias_drift(acc_err["b_corr"], acc_err["b_drift"], n, fs)
    # vibrating acceleration
    acc_vib = np.zeros((n, 3))
    if vib_def is not None:
        if vib_def["type"] == "random":
            acc_vib[:, 0] = vib_def["x"] * np.random.randn(n)
            acc_vib[:, 1] = vib_def["y"] * np.random.randn(n)
            acc_vib[:, 2] = vib_def["z"] * np.random.randn(n)
        elif vib_def["type"] == "sinusoidal":
            acc_vib[:, 0] = vib_def["x"] * np.sin(2.0 * math.pi * vib_def["freq"] * dt * np.arange(n))
            acc_vib[:, 1] = vib_def["y"] * np.sin(2.0 * math.pi * vib_def["freq"] * dt * np.arange(n))
            acc_vib[:, 2] = vib_def["z"] * np.sin(2.0 * math.pi * vib_def["freq"] * dt * np.arange(n))
    # accelerometer white noise
    acc_noise = np.random.randn(n, 3)
    acc_noise[:, 0] = acc_err["vrw"][0] / math.sqrt(dt) * acc_noise[:, 0]
    acc_noise[:, 1] = acc_err["vrw"][1] / math.sqrt(dt) * acc_noise[:, 1]
    acc_noise[:, 2] = acc_err["vrw"][2] / math.sqrt(dt) * acc_noise[:, 2]
    # true + constant_bias + bias_drift + noise
    a_mea = ref_a + acc_bias + acc_bias_drift + acc_noise + acc_vib
    return a_mea


def gyro_gen(fs, ref_w, gyro_err):
    """
    Add error to true gyro data according to gyroscope model parameters
    Args:
        fs: sample frequency, Hz.
        ref_w: nx3 true acc data, rad/s.
        gyro_err: gyroscope error parameters.
            'b': 3x1 constant gyro bias, rad/s.
            'b_drift': 3x1 gyro bias drift, rad/s.
            'arw': 3x1 angle random walk, rad/s/root-Hz.
    Returns:
        w_mea: nx3 measured gyro data
    """
    dt = 1.0 / fs
    # total data count
    n = ref_w.shape[0]
    ## simulate sensor error
    # static bias
    gyro_bias = gyro_err["b"]
    # bias drift Todo: first-order Gauss-Markov model
    gyro_bias_drift = bias_drift(gyro_err["b_corr"], gyro_err["b_drift"], n, fs)
    # gyroscope white noise
    gyro_noise = np.random.randn(n, 3)
    gyro_noise[:, 0] = gyro_err["arw"][0] / math.sqrt(dt) * gyro_noise[:, 0]
    gyro_noise[:, 1] = gyro_err["arw"][1] / math.sqrt(dt) * gyro_noise[:, 1]
    gyro_noise[:, 2] = gyro_err["arw"][2] / math.sqrt(dt) * gyro_noise[:, 2]
    # true + constant_bias + bias_drift + noise
    w_mea = ref_w + gyro_bias + gyro_bias_drift + gyro_noise
    return w_mea


def bias_drift(corr_time, drift, n, fs):
    """
    Bias drift (instability) model for accelerometers or gyroscope.
    If correlation time is valid (positive and finite), a first-order Gauss-Markov model is used.
    Otherwise, a simple normal distribution model is used.
    Args:
        corr_time: 3x1 correlation time, sec.
        drift: 3x1 bias drift std, rad/s.
        n: total data count
        fs: sample frequency, Hz.
    Returns
        sensor_bias_drift: drift of sensor bias
    """
    # 3 axis
    sensor_bias_drift = np.zeros((n, 3))
    for i in range(0, 3):
        if not math.isinf(corr_time[i]):
            # First-order Gauss-Markov
            a = 1 - 1 / fs / corr_time[i]
            b = 1 / fs * drift[i]
            # sensor_bias_drift[0, :] = np.random.randn(3) * drift
            drift_noise = np.random.randn(n, 3)
            for j in range(1, n):
                sensor_bias_drift[j, i] = (a * sensor_bias_drift[j - 1, i] + b * drift_noise[j - 1, i])
        else:
            # normal distribution
            sensor_bias_drift[:, i] = drift[i] * np.random.randn(n)
    return sensor_bias_drift


def gps_gen(ref_gps, gps_err, gps_type=0):
    """
    Add error to true GPS data according to GPS receiver error parameters
    Args:
        ref_gps: If gps_type is 0, [Lat, Lon, Alt, vx, vy, vz], [rad, rad, m].
                 If gps_type is 1, [x, y, z, vx, vy, vz], [m, m, m].
                 ref_gps data are expressed in the navigation frame.
        gps_err: GPS reeceiver parameters.
            'stdp': RMS position error, [m, m, m].
            'stdv': RMS velocity error, [m/s, m/s, m/s].
        gps_type: GPS data type.
            0: default, position is in the form of [Lat, Lon, Alt], rad, m
            1: position is in the form of [x, y, z], m
    Returns:
        gps_mea: ref_gps with error.
    """
    # total data count
    n = ref_gps.shape[0]
    # If position is in the form of LLA, convert gps_err['stdp'] to LLA error
    if gps_type == 0:  # GPS is in the form of LLA, stdp meter to rad
        earth_param = geo_param(ref_gps[0, 1:4])
        gps_err["stdp"][0] = gps_err["stdp"][0] / earth_param[0]
        gps_err["stdp"][1] = gps_err["stdp"][1] / earth_param[1] / earth_param[4]
    ## simulate GPS error
    pos_noise = gps_err["stdp"] * np.random.randn(n, 3)
    vel_noise = gps_err["stdv"] * np.random.randn(n, 3)
    gps_mea = np.hstack([ref_gps[:, 0:3] + pos_noise, ref_gps[:, 3:6] + vel_noise])
    return gps_mea


def odo_gen(ref_odo, odo_err):
    """
    Add error to true odometer data.
    Args:
        ref_odo: nx3, true odometer data, m/s.
        odo_err: odometer error profile.
            'scale': scalar, scale factor error.
            'stdv': scalar, RMS velocity error.
    Returns:
        odo_mea: nx1, measured odometer output.
    """
    n = ref_odo.shape[0]
    odo_mea = np.random.randn(n)
    odo_mea = odo_err["scale"] * ref_odo + odo_err["stdv"] * odo_mea
    return odo_mea


def mag_gen(ref_mag, mag_err):
    """
    Add error to magnetic data.
    Args:
        ref_mag: nx3 true magnetic data, uT.
        mag_err: Magnetometer error parameters.
            'si': 3x3 soft iron matrix
            'hi': hard iron array, [ox, oy, oz], uT
            'std': RMS of magnetometer noise, uT
    Returns:
        mag_mea: ref_mag with error, mag_mea = si * (ref_mag + hi) + noise
    """
    # total data count
    n = ref_mag.shape[0]
    # add error
    mag_mea = ref_mag + mag_err["hi"]
    mag_mea = mag_mea.dot(mag_err["si"].T)
    mag_noise = mag_err["std"] * np.random.randn(n, 3)
    return mag_mea + mag_noise


def geo_param(pos):
    """
    Calculate local radius and gravity given the [Lat, Lon, Alt]
    Local radius include meridian radius rm and normal radius rn.
    Args:
        pos: [Lat, Lon, Alt], rad, m
    Returns:
        rm: meridian radius, m
        rn: normal radius, m
        g: gravity, m/s/s
        sl: sin(Lat)
        cl: cos(lat)
        w_ie: Earth's rotation rate w.r.t the inertial frame, rad/s
    """
    # some constants
    normal_gravity = 9.7803253359
    k = 0.00193185265241  # WGS-84 gravity model constant. For more details, refer to
    # https://en.wikipedia.org/wiki/Gravity_of_Earth
    m = 0.00344978650684  # m = w*w*a*a*b/GM
    # calc
    sl = math.sin(pos[0])
    cl = math.cos(pos[0])
    sl_sqr = sl * sl
    h = pos[2]
    rm = (Re * (1 - E_SQR)) / (math.sqrt(1.0 - E_SQR * sl_sqr) * (1.0 - E_SQR * sl_sqr))
    rn = Re / (math.sqrt(1.0 - E_SQR * sl_sqr))
    g1 = normal_gravity * (1 + k * sl_sqr) / math.sqrt(1.0 - E_SQR * sl_sqr)
    g = g1 * (1.0 - (2.0 / Re) * (1.0 + FLATTENING + m - 2.0 * FLATTENING * sl_sqr) * h + 3.0 * h * h / Re / Re)
    return rm, rn, g, sl, cl, W_IE


def earth_radius(lat):
    """
    Calculate Earth meridian radius and normal radius.
    Args:
        lat: Latitude, rad
    Returns:
        rm: meridian radius, m
        rn: normal radius, m
    """
    sl = math.sin(lat)
    sl_sqr = sl * sl
    rm = (Re * (1 - E_SQR)) / (math.sqrt(1.0 - E_SQR * sl_sqr) * (1.0 - E_SQR * sl_sqr))
    rn = Re / (math.sqrt(1.0 - E_SQR * sl_sqr))
    return rm, rn


def lla2ecef(lla):
    """
    [Lat Lon Alt] position to xyz position
    Args:
        lla: [Lat, Lon, Alt], [rad, rad, meter], numpy array of size (3,)
    return:
        WGS-84 position, [x, y, z], [m, m, m], numpy array of size (3,)
    """
    sl = math.sin(lla[0])
    cl = math.cos(lla[0])
    sl_sqr = sl * sl

    r = Re / math.sqrt(1.0 - E_SQR * sl_sqr)
    rho = (r + lla[2]) * cl
    x = rho * math.cos(lla[1])
    y = rho * math.sin(lla[1])
    z = (r * (1.0 - E_SQR) + lla[2]) * sl
    return np.array([x, y, z])


def lla2ecef_batch(lla):
    """
    [Lat Lon Alt] position to xyz position
    Args:
        lla: [Lat, Lon, Alt], [rad, rad, meter], numpy array of size (n,3)
    return:
        WGS-84 position, [x, y, z], [m, m, m], numpy array of size (n,3)
    """
    # only one LLA
    if lla.ndim == 1:
        return lla2ecef(lla)
    # multiple LLA
    n = lla.shape[0]
    xyz = np.zeros((n, 3))
    for i in range(0, n):
        sl = math.sin(lla[i, 0])
        cl = math.cos(lla[i, 0])
        sl_sqr = sl * sl
        r = Re / math.sqrt(1.0 - E_SQR * sl_sqr)
        rho = (r + lla[i, 2]) * cl

        xyz[i, 0] = rho * math.cos(lla[i, 1])
        xyz[i, 1] = rho * math.sin(lla[i, 1])
        xyz[i, 2] = (r * (1.0 - E_SQR) + lla[i, 2]) * sl
    return xyz


def ecef2lla(xyz):
    """
    [x y z] position in ECEF to [Lat Lon Alt]
    Args:
        WGS-84 position, [x, y, z], [m, m, m], numpy array of size (3,)
    return:
        lla: [Lat, Lon, Alt], [rad, rad, meter], numpy array of size (3,)
    """
    # longitude
    lon = math.atan2(xyz[1], xyz[0])
    # distance from the polar axis
    rho = math.sqrt(xyz[0] * xyz[0] + xyz[1] * xyz[1])
    # Spheroid properties
    b = (1.0 - FLATTENING) * Re  # Semiminor axis
    e2 = FLATTENING * (2.0 - FLATTENING)  # Square of (first) eccentricity
    ep2 = e2 / (1.0 - e2)  # Square of second eccentricity
    # Bowring's formula for initial parametric (beta) and geodetic latitudes
    beta = math.atan2(xyz[2], (1.0 - FLATTENING) * rho)
    lat = math.atan2(xyz[2] + b * ep2 * math.sin(beta) ** 3.0, rho - Re * e2 * math.cos(beta) ** 3.0)
    # Fixed-point iteration with Bowring's formula
    # (typically converges within two or three iterations)
    beta_new = math.atan2((1.0 - FLATTENING) * math.sin(lat), math.cos(lat))
    count = 0
    while count < 5 and beta != beta_new:
        beta = beta_new
        lat = math.atan2(xyz[2] + b * ep2 * math.sin(beta) ** 3.0, rho - Re * e2 * math.cos(beta) ** 3.0, )
        beta_new = math.atan2((1.0 - FLATTENING) * math.sin(lat), math.cos(lat))
        count += 1
    # Ellipsoidal height from final value for latitude
    slat = math.sin(lat)
    N = Re / math.sqrt(1.0 - e2 * slat * slat)
    alt = rho * math.cos(lat) + (xyz[2] + e2 * N * slat) * slat - N
    return np.array([lat, lon, alt])


class GeoMag:
    def __init__(self):
        wmm = []
        r = requests.get("https://blahb.blob.core.windows.net/blahb/WMM.COF", stream=True)
        wmm_file = r.iter_lines()
        for line in wmm_file:
            linevals = line.strip().split()
            if len(linevals) == 3:
                self.epoch = float(linevals[0])
                self.model = linevals[1]
                self.modeldate = linevals[2]
            elif len(linevals) == 6:
                linedict = {
                        "n": int(float(linevals[0])), "m": int(float(linevals[1])), "gnm": float(linevals[2]),
                        "hnm": float(linevals[3]), "dgnm": float(linevals[4]), "dhnm": float(linevals[5]),
                }
                wmm.append(linedict)

        z = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        self.maxord = self.maxdeg = 12
        self.tc = [z[0:13], z[0:13], z[0:13], z[0:13], z[0:13], z[0:13], z[0:13], z[0:13], z[0:13], z[0:13], z[0:13],
                z[0:13], z[0:13], z[0:13], ]
        self.sp = z[0:14]
        self.cp = z[0:14]
        self.cp[0] = 1.0
        self.pp = z[0:13]
        self.pp[0] = 1.0
        self.p = [z[0:14], z[0:14], z[0:14], z[0:14], z[0:14], z[0:14], z[0:14], z[0:14], z[0:14], z[0:14], z[0:14],
                z[0:14], z[0:14], z[0:14], ]
        self.p[0][0] = 1.0
        self.dp = [z[0:13], z[0:13], z[0:13], z[0:13], z[0:13], z[0:13], z[0:13], z[0:13], z[0:13], z[0:13], z[0:13],
                z[0:13], z[0:13], z[0:13], ]
        self.a = 6378.137
        self.b = 6356.7523142
        self.re = 6371.2
        self.a2 = self.a * self.a
        self.b2 = self.b * self.b
        self.c2 = self.a2 - self.b2
        self.a4 = self.a2 * self.a2
        self.b4 = self.b2 * self.b2
        self.c4 = self.a4 - self.b4

        self.c = [z[0:14], z[0:14], z[0:14], z[0:14], z[0:14], z[0:14], z[0:14], z[0:14], z[0:14], z[0:14], z[0:14],
                z[0:14], z[0:14], z[0:14], ]
        self.cd = [z[0:14], z[0:14], z[0:14], z[0:14], z[0:14], z[0:14], z[0:14], z[0:14], z[0:14], z[0:14], z[0:14],
                z[0:14], z[0:14], z[0:14], ]

        for wmmnm in wmm:
            m = wmmnm["m"]
            n = wmmnm["n"]
            gnm = wmmnm["gnm"]
            hnm = wmmnm["hnm"]
            dgnm = wmmnm["dgnm"]
            dhnm = wmmnm["dhnm"]
            if m <= n:
                self.c[m][n] = gnm
                self.cd[m][n] = dgnm
                if m != 0:
                    self.c[n][m - 1] = hnm
                    self.cd[n][m - 1] = dhnm

            # /* CONVERT SCHMIDT NORMALIZED GAUSS COEFFICIENTS TO UNNORMALIZED */
            self.snorm = [z[0:13], z[0:13], z[0:13], z[0:13], z[0:13], z[0:13], z[0:13], z[0:13], z[0:13], z[0:13],
                    z[0:13], z[0:13], z[0:13], ]
            self.snorm[0][0] = 1.0
            self.k = [z[0:13], z[0:13], z[0:13], z[0:13], z[0:13], z[0:13], z[0:13], z[0:13], z[0:13], z[0:13], z[0:13],
                    z[0:13], z[0:13], ]
            self.k[1][1] = 0.0
            self.fn = [0.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0, 11.0, 12.0, 13.0, ]
            self.fm = [0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0, 11.0, 12.0, ]
            for n in range(1, self.maxord + 1):
                self.snorm[0][n] = self.snorm[0][n - 1] * (2.0 * n - 1) / n
                j = 2.0
                # for (m=0,D1=1,D2=(n-m+D1)/D1;D2>0;D2--,m+=D1):
                m = 0
                D1 = 1
                D2 = (n - m + D1) / D1
                while D2 > 0:
                    self.k[m][n] = (((n - 1) * (n - 1)) - (m * m)) / ((2.0 * n - 1) * (2.0 * n - 3.0))
                    if m > 0:
                        flnmj = ((n - m + 1.0) * j) / (n + m)
                        self.snorm[m][n] = self.snorm[m - 1][n] * math.sqrt(flnmj)
                        j = 1.0
                        self.c[n][m - 1] = self.snorm[m][n] * self.c[n][m - 1]
                        self.cd[n][m - 1] = self.snorm[m][n] * self.cd[n][m - 1]
                    self.c[m][n] = self.snorm[m][n] * self.c[m][n]
                    self.cd[m][n] = self.snorm[m][n] * self.cd[m][n]
                    D2 -= 1
                    m += D1

    def GeoMag(self, dlat, dlon, h=0,
            time=date.today()):  # latitude (decimal degrees), longitude (decimal degrees), altitude (meters), date
        # time = date('Y') + date('z')/365
        time = time.year + ((time - date(time.year, 1, 1)).days / 365.0)
        # alt = h/3280.8399
        alt = h / 1000.0  # h was in feet, I changed it to meters, DXG

        otime = oalt = olat = olon = -1000.0

        dt = time - self.epoch
        glat = dlat
        glon = dlon
        rlat = math.radians(glat)
        rlon = math.radians(glon)
        srlon = math.sin(rlon)
        srlat = math.sin(rlat)
        crlon = math.cos(rlon)
        crlat = math.cos(rlat)
        srlat2 = srlat * srlat
        crlat2 = crlat * crlat
        self.sp[1] = srlon
        self.cp[1] = crlon

        # /* CONVERT FROM GEODETIC COORDS. TO SPHERICAL COORDS. */
        if alt != oalt or glat != olat:
            q = math.sqrt(self.a2 - self.c2 * srlat2)
            q1 = alt * q
            q2 = ((q1 + self.a2) / (q1 + self.b2)) * ((q1 + self.a2) / (q1 + self.b2))
            ct = srlat / math.sqrt(q2 * crlat2 + srlat2)
            st = math.sqrt(1.0 - (ct * ct))
            r2 = (alt * alt) + 2.0 * q1 + (self.a4 - self.c4 * srlat2) / (q * q)
            r = math.sqrt(r2)
            d = math.sqrt(self.a2 * crlat2 + self.b2 * srlat2)
            ca = (alt + d) / r
            sa = self.c2 * crlat * srlat / (r * d)

        if glon != olon:
            for m in range(2, self.maxord + 1):
                self.sp[m] = self.sp[1] * self.cp[m - 1] + self.cp[1] * self.sp[m - 1]
                self.cp[m] = self.cp[1] * self.cp[m - 1] - self.sp[1] * self.sp[m - 1]

        aor = self.re / r
        ar = aor * aor
        br = bt = bp = bpp = 0.0
        for n in range(1, self.maxord + 1):
            ar *= aor

            # for (m=0,D3=1,D4=(n+m+D3)/D3;D4>0;D4--,m+=D3):
            m = 0
            # D4=(n+m+D3)/D3
            D4 = n + m + 1
            while D4 > 0:

                # /*
                # COMPUTE UNNORMALIZED ASSOCIATED LEGENDRE POLYNOMIALS
                # AND DERIVATIVES VIA RECURSION RELATIONS
                # */
                if alt != oalt or glat != olat:
                    if n == m:
                        self.p[m][n] = st * self.p[m - 1][n - 1]
                        self.dp[m][n] = (st * self.dp[m - 1][n - 1] + ct * self.p[m - 1][n - 1])

                    elif n == 1 and m == 0:
                        self.p[m][n] = ct * self.p[m][n - 1]
                        self.dp[m][n] = ct * self.dp[m][n - 1] - st * self.p[m][n - 1]

                    elif n > 1 and n != m:
                        if m > n - 2:
                            self.p[m][n - 2] = 0
                        if m > n - 2:
                            self.dp[m][n - 2] = 0.0
                        self.p[m][n] = (ct * self.p[m][n - 1] - self.k[m][n] * self.p[m][n - 2])
                        self.dp[m][n] = (
                                ct * self.dp[m][n - 1] - st * self.p[m][n - 1] - self.k[m][n] * self.dp[m][n - 2])

                # /*
                # TIME ADJUST THE GAUSS COEFFICIENTS
                # */
                if time != otime:
                    self.tc[m][n] = self.c[m][n] + dt * self.cd[m][n]
                    if m != 0:
                        self.tc[n][m - 1] = self.c[n][m - 1] + dt * self.cd[n][m - 1]

                # /*
                # ACCUMULATE TERMS OF THE SPHERICAL HARMONIC EXPANSIONS
                # */
                par = ar * self.p[m][n]

                if m == 0:
                    temp1 = self.tc[m][n] * self.cp[m]
                    temp2 = self.tc[m][n] * self.sp[m]
                else:
                    temp1 = self.tc[m][n] * self.cp[m] + self.tc[n][m - 1] * self.sp[m]
                    temp2 = self.tc[m][n] * self.sp[m] - self.tc[n][m - 1] * self.cp[m]

                bt = bt - ar * temp1 * self.dp[m][n]
                bp = bp + (self.fm[m] * temp2 * par)
                br = br + (self.fn[n] * temp1 * par)
                # /*
                # SPECIAL CASE:  NORTH/SOUTH GEOGRAPHIC POLES
                # */
                if st == 0.0 and m == 1:
                    if n == 1:
                        self.pp[n] = self.pp[n - 1]
                    else:
                        self.pp[n] = ct * self.pp[n - 1] - self.k[m][n] * self.pp[n - 2]
                    parp = ar * self.pp[n]
                    bpp = bpp + (self.fm[m] * temp2 * parp)

                D4 -= 1
                m += 1

        if st == 0.0:
            bp = bpp
        else:
            bp /= st
        # /*
        # ROTATE MAGNETIC VECTOR COMPONENTS FROM SPHERICAL TO
        # GEODETIC COORDINATES
        # */
        bx = -bt * ca - br * sa
        by = bp
        bz = bt * sa - br * ca
        # /*
        # COMPUTE DECLINATION (DEC), INCLINATION (DIP) AND
        # TOTAL INTENSITY (TI)
        # */
        bh = math.sqrt((bx * bx) + (by * by))
        ti = math.sqrt((bh * bh) + (bz * bz))
        dec = math.degrees(math.atan2(by, bx))
        dip = math.degrees(math.atan2(bz, bh))
        # /*
        # COMPUTE MAGNETIC GRID VARIATION IF THE CURRENT
        # GEODETIC POSITION IS IN THE ARCTIC OR ANTARCTIC
        # (I.E. GLAT > +55 DEGREES OR GLAT < -55 DEGREES)

        # OTHERWISE, SET MAGNETIC GRID VARIATION TO -999.0
        # */
        gv = -999.0
        if math.fabs(glat) >= 55.0:
            if glat > 0.0 and glon >= 0.0:
                gv = dec - glon
            if glat > 0.0 > glon:
                gv = dec + math.fabs(glon)
            if glat < 0.0 <= glon:
                gv = dec + glon
            if glat < 0.0 and glon < 0.0:
                gv = dec - math.fabs(glon)
            if gv > +180.0:
                gv -= 360.0
            if gv < -180.0:
                gv += 360.0

        class RetObj:
            pass

        retobj = RetObj()
        retobj.dec = dec
        retobj.dip = dip
        retobj.ti = ti
        retobj.bh = bh
        retobj.bx = bx
        retobj.by = by
        retobj.bz = bz
        retobj.lat = dlat
        retobj.lon = dlon
        retobj.alt = h
        retobj.time = time

        return retobj


def make_sensor(type="90degrees"):
    md = type
    imu_err = "low-accuracy"
    if md == ("90degrees" or "holland_tunnel" or "modest_mouse"):
        imu = IMU(accuracy=imu_err, axis=9, gps=True)
        mdef = md
    else:
        imu = IMU(accuracy=imu_err, axis=9, gps=False)
        mdef = "static"
    sim = Sim([fs, fs_gps, fs_mag], mdef, ref_frame=1, imu=imu)
    sim.run(5)
    gyros = sim.dmgr.get_data_all("gyro")
    accel = sim.dmgr.get_data_all("accel")
    mag = sim.dmgr.get_data_all("mag")
    t = {"gyros": gyros.data[0], "accel": accel.data[0], "mag": mag.data[0]}
    return t
